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ABSTRACT 


An  autonomous  vehicle  must  be  able  to  determine  its  global  position  even  in  the  absence 
of  external  information  input.  To  obtain  reliable  position  information,  this  would  require  the  in¬ 
tegration  of  multiple  navigation  sensors  and  the  optimal  fusion  of  the  navigation  data  provided  by 
them. 

The  approach  taken  in  this  thesis  was  to  implement  two  navigation  sensors  for  a  four-wheel 
drive  and  steer  autonomous  vehicle:  An  inertial  measurement  unit  providing  linear  acceleration  in 
three  dimensions  and  angular  velocity  for  the  vehicle’s  global  motion  and  shaft  encoders  providing 
local  motion  parameters.  An  inertial  measurement  unit  is  integrated  with  the  Shepherd  mobile 
robot  and  data  acquisition  and  processing  software  is  developed.  Position  estimation  based  on  shaft 
encoder  readings  is  implemented.  The  framework  for  future  analysis  including  most  general  motion 
profiles  have  been  laid. 

The  sensor’s  system  performance  was  evaluated  using  three  different  linear  motion  profiles. 
Test  results  indicate  that  the  shaft  encoder  provide  a  positioning  accuracy  better  than  99%  (typ.  7.5 
mm  for  1  m  motion)  under  no  slip  conditions  for  pure  translational  motion.  The  IMU  still  requires 
further  improvement  to  allow  for  both  sensors  to  be  combined  to  an  integrated  system. 
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I. 


INTRODUCTION 


A.  BACKGROUND  AND  MOTIVATION 

Landmines  have  become  an  ever  increasing  threat  for  the  civilian  communities  in  post-war 
scenarios.  Several  million  land  mines  are  scattered  around  the  world  annually  causing  more  than 
10,000  fatalities  and  more  than  20,000  severe  injuries  to  non-combattants. 

Since  effective  multi-national  proliferation  treaties  banning  the  use  of  anti-personnel  mines 
are  not  yet  in  place  and  with  major  producers  for  those  mines  not  likely  to  sign  these  treaties  because 
of  their  important  impact  on  conventional  warfare,  it  is  essential  to  develop  and  deploy  equipment 
for  detection  of  anti-personnel  mines  in  mine-contaminated  regions. 

Moreover,  many  countries  are  downsizing  their  armed  forces  due  to  budget  constraints  and 
thus  turning  over  formerly  used  defense  sites  to  the  local  communities.  Wide  areas  of  these  defense 
sites  (such  as  proving  ground,  rifle  ranges,  ...)  are  contaminated  with  unexploded  ordnance  (UXO). 
The  contaminated  land  must  be  cleared  before  transferring  to  civilian  use. 


B.  OBJECTIVE 

At  present,  there  are  not  many  effective  means  for  mine  and  UXO  detection  available. 
The  current  approach  to  mine  and  UXO  detection  and  clearance  is  labor  and  time  intensive  and 
dangerous:  explosive  ordnance  disposal  (EOD)  personnel  walks  slowly  over  the  area  that  is  to  be 
cleared,  trying  to  detect  buried,  half  buried  or  totally  exposed  material.  Once  an  object  is  found, 
successive  steps  in  the  clearance  process  would  include: 

•  detect, 

•  identify, 

•  excavate, 

•  defuse, 

•  transport 
and 

•  dispose 

the  object  in  question.  It  is  therefore  desireable  to  develop  a  robust,  low-cost  tool  for  persuing 
the  above  steps  through  the  use  of  robotics  and  advanced  sensing  techniques  meeting  the  following 
requirements: 
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•  Robustness  for  operation  in  rough  terrain 

•  Expandability  for  different  sensors  and  equipment 

•  Precise  navigation  tools 

Multi-disciplinary  research  conducted  in  the  Departments  of  Electrical  and  Computer  Engi¬ 
neering,  Computer  Science,  Aeronautics  and  Astronautics,  and  the  Physics  Department  at  the  Naval 
Postgraduate  School,  centers  around  the  development  of  a  semi-autonomous  robot  system  for  land 
mine/UXO  searching/processing  tasks  in  humanitarian  operations  [2],  This  project  has  required  the 
cooperative  effort  of  several  NPS  thesis.  The  emphasis  of  this  thesis  is  the  implementation  of  an 
integrated  navigation  system.  In  the  long  term,  the  system  components  will  be  comprised  by  a  land 
vehicle,  an  aerial  vehicle,  and  a  ground-based  control  center. 

The  land  vehicle,  specifically  designed  for  the  aforementioned  tasks  is  four-wheel  steerable 
and  drivable.  A  prototype  vehicle  called  SHEPHERD  is  currently  in  use  for  this  research  project. 
The  unique  design  of  SHEPHERD  provides  a  high  level  of  sophistication  for  motion  control  for  it 
to  be  able  to  precisely  traverse  rough  terrain.  The  interested  reader  is  referred  to  [1].  The  scope  of 
this  project,  in  general,  is  very  comprehensive  and  encompasses  many  scientific  areas.  In  particular, 
interdisciplinary  tools  such  as  physics  principles  including  coordinate  transformations,  kinematics 
and  mechanics  of  rigid  bodies,  and  electrical  and  software  engineering  tools  are  used,  discussed  and 
covered  in  this  thesi;-:. 

In  order  to  control  the  vehicle  and  implement  efficient  search  patterns  while  at  the  same  time 
reducing  redundant  search  paths,  precise  knowledge  of  the  vehicle’s  velocity  and  position  is  essential. 
Using  an  on-board  inertial  navigation  system,  the  vehicle’s  acceleration  can  be  measured  and  it’s  3D 
motion  precisely  computed  by  the  on-board  computer.  However,  an  inertial  sensor  alone  can  provide 
accurate  position  information  only  in  the  short  term,  but  must  be  integrated  with  additional  sensors 
if  precise  long  term  positional  data  is  required.  The  vehicle’s  rough  operation  environment  makes 
it  essential  that  extremeley  accurate  position  information  is  obtained.  To  meet  this  requirement,  a 
Global  Positioning  System  (GPS)  receiver  shall  be  integrated. 

The  purpose  of  this  thesis  is  to  implement  and  evaluate  an  integrated  navigation  system  for 
SHEPHERD  enabling  the  operation  of  the  vehicle  under  extremely  rough  conditions  while  at  the 
same  time  providing  accurate  position  information.  This  thesis  will  examine  the  following  research 
questions: 

1.  Provide  the  theoretical  background  for  coordinate  transformations, 

2.  Implement  the  hardware  and  software  for  an  Inertial  Measurement  Unit  (IMU), 

3.  Implement  the  software  to  determine  position  based  on  the  on-board  shaft  encoders. 
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4.  Develop  a  scheme  for  sensor  fusion  for  slip-detection. 


C.  ORGANIZATION 

First,  a  brief  overview  of  the  computer  architecture  for  the  Shepherd  Rotary  Vehicle  is  given 
in  Chapter  11.  Secondly,  Chapter  III  defines  the  basic  reference  frames  that  are  being  used  throughout 
this  project.  The  secondary  means  of  determining  the  vehicle  motion  is  given  by  shaft  encoders 
that  are  used  for  each  of  four  wheels  for  both,  steering  and  driving.  The  software  implementation  is 
described  in  Chapter  IV.  Chapter  V  describes  the  implementation  of  a  low  cost  inertial  measurement 
system  (IMS)  both  in  hardware  and  software.  Its  purpose  will  be  to  complement  the  shaft  encoder 
system  in  situations  were  slip  occurs.  How  both  systems  may  be  unified  for  slip  detection  and  to 
further  improve  the  performance  of  the  navigation  system  is  investigated  in  Chapter  VI.  Finally, 
the  success  and  limitations  of  the  use  of  the  system  described  herein  is  summarized  in  Chapter  VII 
providing  essential  results  of  this  research  and  recommendations  for  future  research  in  this  area. 
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II. 


SYSTEM  OVERVIEW 


In  this  chapter  we  will  give  a  brief  computer  hardware  description  of  the  system  configuration 
for  the  SHEPHERD  Rotary  Vehicle.  This  complements  the  description  given  by  Mays/Reid  [1]  and 
is  intended  to  provide  the  essential  information  necessary  to  understand  the  cross-references  to 
computer  components  given  in  the  following  Chapters. 

All  mechanical  information  for  the  mobile  platform  is  extensively  discussed  by  Mays/Reid 
[1].  However,  we  shall  note  at  this  point  that  the  Shepherd  Rotary  Vehicle  is  a  four  wheel  drive 
and  steer  mobile  robot.  The  four  wheels  are  steerable  without  limitations  and  can  be  rotated  and 
driven  in  either  direction  (more  than  360  degree  of  rotation  space).  The  four  wheel  drive  and 
steer  capability  shall  provide  the  robustness  required  for  operation  in  rough  terrain  (e.g,,  sand  dune 
scenarios,  ...).  A  side  view  and  front  view  photo  taken  from  SHEPHERD  with  a  digital  camera  are 
shown  in  Figure  2.1  and  Figure  2.2,  respectively. 

In  Figure  2.1  we  can  can  see  the  four  suspension  boxes  for  the  four  wheels,  the  steel  plate 
that  comprises  the  main  support  frame  for  the  robot,  the  inertial  measurement  sensor  mounted 
upside  down  below  the  steel  plate,  and  a  joystick  that  is  used  to  manually  steer  the  robot  in  the 
top  right-hand  corner.  In  addition,  in  the  rear  view  photo  you  can  see  the  Laptop  computer,  to  its 
left  a  switchbox  for  connecting  the  Laptop  to  either  a  CONSOLE  or  HOST  serial  port,  and  to  its 
right  the  joystick.  Another  view,  shown  in  Figure  2.3  shows  the  Laptop  placed  on  the  steel  plate 
and  behind  it  the  servo  control  rack  and  the  VMEBus  chassis. 

The  complete  hardware  architecture  is  comprised  of  the  TAURUS  Single  Board  Computer 
[3],  a  VME-Bus  based  single  board  computer  with  a  Motorola  MC68040  as  main  processor  and 
several  other  on-board  processing  components  and  the  VME-Bus.  At  present,  this  stand  alone 
computer  system  is  expanded  with  a  servo  controller  unit  that  interfaces  to  the  four  wheels  and 
a  16-channel  differential  input  A/D-Board.  Four  channels  of  the  A/D-Board  are  utilized  for  the 
inertial  measurement  unit  (IMU)  which  is  discussed  in  Chapter  V.  In  the  future,  the  system  may 
be  expanded  with  several  other  sensors  through  the  use  of  the  VME-Bus,  Figure  2.4  shows  a  block 
diagram  of  the  system  configuration  for  SHEPHERD. 


A.  TAURUS  BOARD 

This  section  describes  the  TAURUS  single-board  computer  system’s  main  features.  The 
hardware  is  based  on  a  dual  processor  platform  using  Motorola’s  68040  as  the  main  processor  and 
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Figure  2.1:  Side  view  from  the  SHEPHERD  Rotary  Vehicle. 


Figure  2.2:  Rront  view  from  the  SHEPHERD  Rotary  Vehicle  with  wheels  rotated  by  45°. 
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Figure  2.3:  Top  view  from  the  SHEPHERD  Rotary  Vehicle.  In  the  front,  the  Pentium  Laptop  used 
as  a  concole,  in  the  middle  the  servo  controller  chassis,  and  in  the  back  the  VMEBus  rack. 


7 


>  S  W  ffl  3 


Pl-Connector 


P2-Connector 


TAURUS  Board 


MC68C681 

■ 

Port  A 

Port  B 

CD2401 

1 

Port  1 

Port  2 

Port  3 

Port  4 

S2C55  Dev.l 

Port  A  * 

Port  B 

Port  C  - 

UONSOL^ 


UNIX 

Workstation 


GPS  Receiver 


Joystick 


y-Position 


Button-Status 


Interface  Card 
(10  Bit  A/D) 


^  Pl-Connector 


AVME-9325-5  A/D-Board 


4  Channels/12  Bit 


MotionPak 


Pl-Connector 


VIPC-610  (Industry  Pack  Carrier) 

IP-Quadrature  2 

IP-Quadrature  1 

24Bit/CH4..7 


24Bit/CH0..3 


Steer  Wheel  1..4 


Drive  Wheel  1..4 


Pl-Connector 


VME-9210  Analog  Out 


Velocity  Control 
(12  Bit, 
8-Channels) 


Steer  Wheel  1..4 


Drive  Wheel  1..4 


Pl-Connector 


VME-2170A  Digital  Out 


32  Bit  Digital  out 


Bits  12. .23 


-Connector 


Digital  in 


Bits  0..11 

Bits  12. .23 


Steer  Wheel  1..4 


Drive  Wheel  1..4 


Bits  24. .31 


Steer  Wheel  1,.4 


Drive  Wheel  1..4 


Figure  2.4:  Shepherd  Rotary  Vehicle  Hardware  Configuration. 
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the  68030  as  a  slave  processor  for  basic  I/O  functions.  Signaling  between  both  processors  takes  place 
via  processor  interrupts.  The  system  is  attached  to  a  VME  bus  backplane  providing  the  capability 
to  expand  the  system  as  far  as  main  memory  and  additional  sensor  devices  are  concerned.  Among 
the  many  I/O  functions  that  the  TUARUS  board  provides  are: 

•  six  RS-232C  serial  communication  ports  (two  through  a  DUART  68C681,  and  four  through 
a  CD2401  Communications  Device) 

•  two  24  bit  parallel  ports 

•  several  timer /counters:  Five  provided  by  the  AM9513A  System  Timing  Controller,  one 
timer  is  provided  in  the  68C681  serial  port  device  and  eight  timer/counters  are  available  in 
the  CD2401 

•  real  time  calendar  clock  device  MK48T08 

•  SCSI  Port 

•  Ethernet  Port 

More  information  can  be  obtained  from  [3]  and  the  respective  operating/user  manuals  for 
each  device.  Rather  than  focussing  on  all  the  technological  aspects  for  each  device,  we  merely  focus 
on  those  important  ones  for  understanding  the  operation  of  SHEPHERD. 


1.  TAURUS  Bug  Monitor/ Debugger 

For  start-up  and  debugging/monitoring  purposes,  a  debugger /monitoring  program  called 
TAURUSBug  resides  in  the  memory  region  from  Oxff 800000  through  Oxff 9f ff ff  (memory  bank 
2,  see  [3],  Chapter  2.2).  The  user  may  decide  whether  or  not  to  use  this  program  for  the  boot-up. 
However,  in  the  sequel,  the  project  group  has  made  heavy  use  of  the  debugging  tools  provided  by 
TAURUSBug. 


2.  DUART  68C681 

The  TAURUS  board  features  a  68C681  device  which  provides  two  dual  asynchronous  re¬ 
ceiver/transmitter  (DUART)  serial  ports  with  RS-232C  interface.  These  two  ports  are  utilized  for 
up-/and  downloading  of  executable  code  and  data  and  for  user  interaction  with  SHEPHERD.  Port 
A  is  called  CONSOLE  and  Port  B  is  called  HOST.  Both  ports  are  connected  through  a  switchbox 
to  the  laptop  computer. 
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3. 


Cirrus  Logic  Communications  Controller  CD 2401 


Up  to  date,  only  one  of  the  four  RS-232C  serial  ports  provided  by  the  Cirrus  Logic  Com¬ 
munications  Controller  CD2401  is  used  for  interfacing  the  GPS  receiver. 


4.  AM9513A  Counter/Timer 

The  AM9513A  LSI  circuit  provides  a  total  of  five  independent  16-bit  timer /counters  which 
can  be  cascaded  to  a  single  80-Bit  timer/counter  for  long-term  observations.  The  timer  number  five 
is  used  for  deriving  the  motion  control  clock  of  T=10  ms:  every  10  ms  a  timer  interrupt  is  issued  to 
trigger  another  motion  control  cycle.  This  10  ms  timer  interrupt  is  clearly  the  heart  of  the  system. 
Care  should  be  taken  that  this  interrupt  is  granted  the  highest  priority  level  available.  This  leads 
to  the  decision  to  use  timer  five  instead  one  of  the  other  four. 

5.  Programmable  Parallel  I/O  Port  Device  (Intel  82C55A) 

The  Taurus  board  is  equipped  with  two  Intel  82C55A  devices  each  providing  three  8-Bit  wide 
ports  (Port  A,  B,  and  C).  Only  the  first  device  is  currently  in  use  for  the  motion  control  by  means 
of  a  joystick.  A  simple  PCB  board  interfaces  an  IBM-PC  Joystick  to  this  I/O  device.  However, 
some  minor  changes  to  the  layout  of  the  Joystick  circuitry  had  to  be  made.  Port  A  comprises  the 
x-Position  (an  8-bit  digital  value  ranging  from  0  ...  255  equivalent  to  joystick  left  to  right).  Port  B 
gives  the  y-Position  in  the  range  0  ...  255  equivalent  to  down  (0x00)  and  up  (Oxff).  Currently,  only 
Bits  zero  and  one  are  in  use  from  Port  C  providing  status  information  for  the  two  switches  on  the 
throttle  (pushing  the  left  switch  or  the  center  switch  on  the  trottle  will  set  bit  zero  and  pressing  the 
right  button  on  the  throttle  will  set  bit  one).  The  other  two  push  buttons  on  the  left-hand  side  of 
the  joystick  have  currently  no  function.  In  case  that  needed,  they  can  easily  be  connected  to  any  of 
the  six  remaining  bits  of  Port  C  through  the  PCB  board  by  use  of  pull-up  resistors. 


6.  Interrupts 

Both  on-board  and  off-board  Interrupts  are  supported  by  the  TAURUS  board.  All  on-board 
Interrupts  are  routed  through  the  Interrupt  Steering  Mechanism  (ISM)  to  either  the  68030  I/O 
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Processor  or  via  a  VMEbus  Interface  Controller  device  (VIC068)  to  the  68040  Processor.  Note  that 
an  interrupt  can  only  be  routed  to  one  processor  at  a  time.  The  VIC068  guides  both,  ISM  interrupts 
and  VMEbus  interrupts  to  the  68040  processor.  This  is  depicted  by  Figure  2.5.  In  accordance  with 
[3],  the  local  interrupts  by  on-board  sources  from  the  ISM  to  the  VIC  will  be  labelled  as  LIRQ-x 
whereas  the  interrupts  form  the  VIC068  to  the  68040  processor  are  labelled  IRQ-x. 


Interrupts 

from 

On-Board 

Devices 


VMEbus  Interrupts 


Figure  2.5:  Servicing  of  on-board  Interrupts  or  off-board  VME-Bus  Interrupts  (From  Ref.  [3]) 

The  ISM  combines  groups  of  on-board  Interrupts  to  act  as  a  single  interrupt  source  towards 
either  the  68030  or  68040  processor.  It  is  important  to  note  that  the  VIC068  device  enables  the 
programmer  to  shift  the  interrupt  levels.  In  order  to  handle  the  proper  handshaking  in  this  case, 
the  appropriate  LIRQ-Shift-Register  in  the  ISM  would  have  to  be  set.  The  TAURUS  user’s  manual 
[3]  p.  2-71  gives  the  following  example: 

...  if  LIRQ-5  from  the  ISM  is  shifted  in  the  VIC068  to  IRQ-3,  then  the  acknowledge  signal 
from  the  68040  processor  to  the  VIC068  would  be  IACK-3  which  would  be  passed  on  to  the 
ISM  device.  LIRQ-SR5  (at  $FFF4800A  -  upper  nibble)  would  be  set  to  shift  [the]  VIC068 
IACK-3  input  to  output  ISM-IACK-5. 

Some  facts  that  should  be  remembered: 

•  each  Interrupt  group  is  associated  with  an  ISM  Configuration  Register  Nibble. 

•  the  MSB  of  each  Nibble  is  the  steering  Bit,  where  ‘0’  routes  the  interrupt  to  the  68030. 

•  the  remaining  three  bits  of  each  nibble  encode  the  local  interrupt  level. 

•  upon  Power-Up  or  RESET,  all  On-Board  Interrupts  are  disabled. 

•  Taurus  Vector. Table  Base  address  is  at  OxffelOxxx  where  xxx  =  4  x  Vector  Number. 
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B. 


MOTION  CONTROL 


As  indicated  in  the  previous  section,  a  motion  control  cycle  is  initiated  with  every  10  ms 

timer  interrupt.  In  brief,  this  motion  control  cycle  is  given  by  the  following  sequence  of  logical 
blocks: 


readEncoderO 

computeRatesO 

bodyMotionO 

wheelMotionO 

driveMotors  0 


Read  ail  shaft  encoders 

Compute  (angular)  velocity  for  all  steering  and  driving  motors 
Compute  motion  parameters  for  the  vehicle’s  body  (bodyMotion) 
Compute  the  angles  and  speeds  required  for  each  wheel  based  on 
the  results  of  bodyMotion 

Update  the  servos  for  driving  and  steering  motors 


The  organization  of  the  motion  control  cycle  is  described  in  more  detail  in  Mays/Reid  [1], 
However,  it  should  be  noted  that  the  source  code  as  given  there  has  been  modified  slightly  to  make 
the  routines  more  efficient  and  thus  less  time  consuming. 
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III. 


REFERENCE  FRAMES 


This  chapter  gives  a  brief  discussion  on  reference  frames  that  are  being  used  throughout 
this  thesis. 


A.  BODY  MOTION 

In  the  analysis  of  the  motion  of  a  rigid  body,  it  turns  out  that  considerable  simplification 
in  the  mathematical  formulas  for  rigid-body  motion  can  be  reached  if  the  motion  is  described  with 
respect  to  its  principal  axes.  The  principal  axes  are  chosen  such  that  the  cross  terms  (sometimes 
called  the  products  of  inertia)  of  the  moment  of  inertia  tensor  I  vanish  (see  [4]  for  a  more  detailed 
analysis  of  this).  The  axes  form  a  right-handed  coordinate  system  with  the  origin  usually  taken  to 
be  at  the  body’s  center  of  mass  (CM).  However,  at  this  point  we  are  not  concerned  with  the  moment 
of  inertia  tensor. 


1.  Body  Reference  Frame 


For  the  purpose  of  describing  the  kinematics  of  a  body  moving  on  the  Earth’s  surface  the 
reference  frame  is  chosen  such  that  axes  of  the  body  frame,  which  we  will  call  fi:ame  {B},  are  given 
by  the  principal  axes  of  the  body  given  as  follows: 


X  -  longitudinal  axis  (oriented  from  rear  to  front  of  body) 
y  -  transversal  axis  (oriented  to  the  left) 

z  -  normal  axis  (oriented  pointing  up,  away  from  the  center  of  the  Earth) 


2.  Sensor  Reference  Frame 

Sensors  will  be  employed  with  a  vehicle  in  order  to  measure  parameters  pertaining  to  the 
vehicle’s  kinematics.  The  sensor  will  provide  data  relative  to  its  own  frame,  which  we  will  call  sensor 
frame  {S}.  In  general,  this  frame  can  be  completely  different  from  the  body  frame.  If  sensing  data 
is  provided  in  a  Cartesian  coordinate  system,  the  only  difference  between  {B}  and  {S}  might  be  an 
offset  (or  translational  difference)  ®Ps,or5* 
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3. 


Earth  Reference  Frame 


In  order  to  express  the  motion  of  a  body  as  observed  by  an  outside  inertial  observer  we  need 
to  define  a  suitable  inertial  reference  frame.  An  inertial  reference  frame  is  defined  to  be  the  frame 
for  which  Newton’s  laws  of  motion  are  valid.  For  a  slow  moving  vehicle  at  a  particular  point  on  the 
Earth’s  surface,  a  suitable  reference  frame  {R}  is  set  up  in  the  following  way: 

X  -  pointing  north  ““  ^ 

y  -  pointing  east 

z  -  pointing  down,  towards  the  center  of  the  Earth 

We  will  see  later  in  this  chapter  that  the  axes  x,y  and  z  of  this  coordinate  system  refer  to  the  geodetic 
descriptions  of  latitude,  longitude  and  geodetic  height  respectively.  Since  we  do  not  anticipate  any 
large  scale  motion  (  on  the  order  of  kilometers  )  it  is  sufficient  not  to  concern  ourselves  with  the 
irregular  shape  of  the  Earth  and  with  the  resulting  mapping/projection  problems. 


B.  GPS  SYSTEM 

In  order  to  describe  both  the  GPS  Satellite  motion  and  receiver  motion,  it  is  necessary  to 
choose  a  common  reference  system.  Most  commonly,  the  motion  is  described  in  terms  of  velocity 
and  position  as  measured  in  a  Cartesian  Coordinate  System.  The  most  applicable  coordinate  system 
for  GPS  systems  are  given  as  follows:  Satellite  and  GPS  receiver  motion  are  described  in  terms  of 
the  Earth-Centered  Inertial  and  Earth-Centered  Earth-Fixed  coordinate  systems  respectively.  The 
systems  in  use  are  described  in  detail  by  Kaplan  [5]  and  are  briefly  explained  below: 


1.  Earth-Centered  Inertial  (ECI)  Coordinate  System 

In  this  system,  the  origin  is  the  center  of  mass  of  the  Earth.  Satellites  orbiting  the  Earth 
obey  Newton’s  second  law  of  motion  as  described  in  this  System.  In  the  ECI  system,  the  xy-plane 
coincides  with  the  Earth’s  equatorial  plane,  the  +x-axis  points  towards  some  fixed  point  in  space 
(celestial  sphere),  the  z-axis  is  taken  to  be  normal  to  the  xy-plane  pointing  towards  the  north  pole. 
The  set  of  axis  forms  a  right-handed  coordinate  system.  However,  due  to  the  Earth’s  inhomogeneous 
shape,  irregularities  in  the  Earth’s  motion  cause  the  ECI  frame  not  to  be  truly  inertial.  Therefore, 

the  GPS  system  defines  the  ECI  reference  frame  as  given  by  the  constellation  at  1200  hr  UTC  on 
January  1,  2000. 
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2. 


Earth-Centered  Earth-Fixed  (ECEF)  Coordinate  System 


For  computing  the  receivers  position,  it  is  more  convienient  to  use  a  system  that  is  stationary 
in  the  earth  frame.  It  is  known  as  Earth- Centered  Earth-Fixed  (ECEF).  As  with  the  ECI  frame, 
the  xy-plane  is  coincident  with  the  Earth’s  equatorial  plane,  the  x-axis  points  in  the  direction  of  0° 
longitude,  the  y-axis  points  in  the  direction  of  90°  longitude.  The  x-  and  y-axes  therefore  no  longer 
describe  fixed  directions  in  inertial  space.  The  z-axis  completes  the  right-handed  coordinate  system. 


3.  Conversion  between  ECI  and  ECEF 

Conversions  between  ECI  and  ECEF  system  are  accomplished  by  means  of  matrix  trans¬ 
formations  (rotator  matrices)  which  are  not  further  described  in  this  thesis.  It  is  assumed  that  the 
Satellite  ephemeris  data  is  already  translated  into  ECEF  system. 


4.  World  Geodetic  System  (WGS-84) 

The  Department  of  Defense  invented  a  system  to  model  all  irregularities  pertaining  to  de¬ 
scribing  the  Earth’s  gravitational  motion.  This  system  is  known  as  the  World  Geodetic  System 
(WGS-84).  In  addition  to  modeling  the  gravitational  irregularities,  the  World  Geodetic  System 
provides  an  ellipsoidal  model  of  the  Earth.  The  ECEF  coordinate  system  is  affixed  to  the  World 
Geodetic  System  reference  ellipsoid  and  thus,  latitude,  longitude  and  height  of  a  receiver  can  be 
specified  with  respect  to  this  ellipsoid. 


C.  TRANSFORMATIONS 

To  define  and  manipulate  physical  quantities  such  as  acceleration,  velocity  and  position  we 
must  define  coordinate  systems  and  find  transformations  for  describing  vectors  given  in  one  system 
with  respect  to  the  other.  These  transformations  will  be  accompanied  by  conventions  for  their 
representation. 

A  great  variety  of  similar  transformations  can  be  found  in  many  textbooks.  Not  all  of 
them  are  concisely  formulated.  It  is  thus  rather  confusing  to  relate  different  conventions  given  in 
different  textbooks  with  each  other;  even  though  they  may  describe  the  same  transformation.  A 
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good  introduction  on  spatial  descriptions  and  transformations  is  given  by  [6]  and  we  will  therefore 
briefly  outline  the  most  important  aspects  and  conventions  as  thev  pertain  to  our  problem. 

The  inertial  reference  frame  {R}  is  given  by  the  set  of  coordinate  axis  {x,y,z}  where  the 
xy-plane  is  the  plane  parallel  to  the  WGS-84  reference  ellipsoid  (that  is,  the  earth’s  surface)  with  x 
pointing  north,  y  pointing  east  and  z  pointing  towards  the  geodetic  center  of  the  Earth.  The  frame 
{B}  which  is  attached  to  the  body  is  given  by  the  set  of  axes  {  x',y',z'  }  with  x'  pointing  forward, 
y'  pointing  to  the  left  of  the  body  and  z'  completing  the  right-handed  coordinate  system.  Figure 
3.1  shows  both  frames. 


Figure  3.1.  Coordinate  FVame  for  Body  relative  to  point  on  Earth  surface.  The  x/y-plane  spans  the 
plane  tangent  to  the  Earth’s  surface. 

There  are  two  governing  basic  methods  of  representing  the  orientation  of  a  body  (with  the 
Frame  {B}  attached  to  it)  with  respect  to  the  reference  frame  {R}.  One  way  is  to  express  the 
principal  directions  of  {B}  (unit  vectors  x',y',z')  in  terms  of  the  coordinate  system  {R}  and  stack 
these  three  unit  vectors  together  as  the  columns  of  a  3  x  3  proper  orthonormal  rotation  matrix 

^R=[xyz'] 

where  ^  R  has  the  properties  that  its  columns  are  mutually  orthogonal  and  have  unit  length  and 
det{^  R)  =  1.  Moreover,  it  can  be  shown  that  the  inverse  of  “  R  is  simply  its  transpose: 
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and  thus  giving  rise  to 


Rt}  R-o— 1  R-U  RtjT  r 

gJXgJX  gJXgJx  —  1 

Any  vector  P  given  with  respect  to  {B}  can  then  be  expressed  in  terms  of  {R}  by  the  transformation 

^P  =  ^R®P 

Since  dealing  with  3x3  matrices  for  describing  orientations  is  usually  very  tedious,  a  second  way 
of  describing  the  orientation  of  a  body  can  be  derived  from  a  result  from  linear  algebra.  Cayley’s 
formula  for  orthonormal  matrices  (cited  by  Craig  [6])  states  that  any  3x3  orthonormal  matrix  can 
be  specified  by  just  three  parameters. 

There  are  many  ways  to  represent  orientations  with  only  three  parameters.  Not  all  of  them 
are  convenient  and  the  reader  may  be  easily  confused  while  looking  for  those  in  different  textbooks. 
In  the  discussion  here  we  follow  the  conversion  of  Ref.  [6] . 


1.  Roll,  Pitch,  and  Yaw 

One  way  of  describing  the  orientation  of  a  frame  {B}  relative  to  the  reference  frame  {R} 
is  by  describing  the  body’s  orientation  by  observing  successive  rotations  about  the  three  axes  (x,y, 
and  z)  of  the  fixed  refernece  frame  {R}.  Craig  [6]  refers  to  this  convention  as  X-Y-Z  fixed  angles: 

1.  start  with  the  frame  {B}  coincident  with  the  reference  frame  {R} 

2.  rotate  {B}  about  ^x  by  the  roll  angle  0 

3.  rotate  {B}  about  ^y  by  the  pitch  angle  <{> 

4.  rotate  {B}  about  by  the  yaw  angle  ^ 

Each  of  the  three  rotations  takes  place  about  an  axis  in  the  fixed  reference  frame  {R}.  The  resulting 
rotation  matrix  can  be  obtained  by  successively  rotating  the  frame  {B}  about  single  axes  in  the 
stationary  frame  {R}: 

bR 

where 

^Ry{4>) 


=  '‘Rz(tA)  “Ry(.A) 


coa{'4>)coa{<i>)  co5(V')«tn(«^)jin(0)  —  5tn(V')co«(d)  co5(-0)A>n(^)cod(0)  +  aiTi{V')a»Ti(^) 

ain{il^)coa{4>)  a»n(V>)a**i(^)ain{0)  -t"  coa{ift)coa{9)  ain(V')a*«(^)co5(0)  —  ) 

—  ain(4>)  coa{<f^)ain(6)  coa(4>)coa(B) 


(3.2) 


1  0 

0  CO3(0) 

0  3in(0) 


coa(4>) 

0 

-ain{<t>) 


0 

— 3in(0) 
C03(fl) 

0 

co«(^) 


(3.3) 

(3.4) 
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'‘Rz(V-) 


0 

0 

1 


(3.5) 


ain(V’)  coj(V>) 


0 


Therefore,  a  vector  given  in  frame  {B}  can  be  transformed  with  respect  to  frame  {R}  by  the 
transformation 


"a  =  ^R®a 


2.  Euler  Angles 

Another  possible  description  of  the  frame  {B}  with  respect  to  frame  {R}  is  given  by  the 
Euler  Angles.  As  opposed  to  rotating  the  frame  {B}  in  successive  steps  about  the  fixed  axes 
of  {R},  this  description  will  involve  successive  rotations  performed  about  the  principal  axes  of  the 
rotating  frame  {B}  we  are  about  to  move: 

1.  start  with  the  frame  {B}  coincident  with  the  reference  frame  {R} 

2,  rotate  {B}  about  ®z  by  the  angle  ^ 

3.  rotate  {B}  about  by  the  angle  0 

4,  rotate  {B}  about  by  the  angle  6 

The  resulting  rotation  matrix  is  the  same  as  given  above  in  Equation  3.2.  Instead  of  naming  the 
angles  as  roll,  pitch,  and  yaw  respectively,  they  are  now  being  referred  to  as  the  Euler  Angles. 

Craig  refers  to  them  as  the  Z-Y-X  Euler  Angles.  This  transformation  is  equivalent  to  the  one 
given  by  Fossen  [7]  on  page  10  except  that  we  exchanged  the  naming  for  roll  and  pitch  {9  ^). 

The  result  obtained  yields  a  fundamental  statement  as  given  by  Craig  [6]: 

...  three  rotations  taken  about  fixed  axis  yield  the  same  final  orientation  as  the  same  three 
rotations  taken  in  opposite  order  about  the  axes  of  the  moving  frame. 

In  this  work,  we  will  make  reference  to  the  Eulerian  angles  and  this  mostly  to  the  fact  that 
the  Eulerian  angles  are  easier  to  recognize.  However,  the  euler  angles  are  equivalent  to  the  roll,  yaw 
and  pitch  angles. 

In  this  chapter  we  have  laid  the  framework  for  transforming  vectors  from  one  coordinate 
system  to  the  other.  We  will  apply  this  to  the  Inertial  Measurement  Unit  and  develop  a  scheme 
for  determining  the  specific  acceleration  acting  on  a  body  even  in  the  presence  of  the  gravitational 
acceleration. 
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IV.  POSITION  DETERMINATION  WITH  SHAFT 

ENCODER 


This  chapter  describes  the  use  of  the  shaft  encoders  for  position  determination.  It  comple¬ 
ments  and  in  some  cases  alters  the  results  obtained  by  Mays/Reid  [1].  As  outlined  in  Mays/Reid  [1], 
each  servo  motor  is  equipped  with  shaft  encoders  which  record  the  actual  angles  for  all  eight  motors. 
This  should  provide  an  easy  means  for  direct  position  determination  under  the  condition  that  no 
slip  occurs.  That  is,  the  difference  between  an  interval  T=10  ms  by  which  each  encoder  (driving  and 
steering)  advances  is  directly  proportional  to  the  distance  travelled  or  to  the  angle  each  wheel  was 
rotated  and  accordingly  for  the  time  of  observation  proportional  to  the  linear  and  angular  velocity. 

It  should  be  noted  that  the  shaft  encoders  for  the  driving  motors  count  positive  for  a 
clockwise  rotation  of  the  wheel.  Thus,  if  all  wheels  are  driving  forward  (which  implies  that  wheels  1 
and  3  are  commanded  with  negative  servo  data)  the  shaft  encoder  readings  will  decrease  for  wheels 
2  and  4.  In  the  same  manner,  if  all  wheels  are  steering  to  the  right  (clockwise  as  viewed  from  above, 
with  negative  servo  data  commanded),  the  shaft  encoder  readings  will  increase  for  all  wheels. 


A.  DETERMINING  THE  SERVO  PARAMETERS 


It  might  be  necessary  from  time  to  time  to  verify  and  adjust  the  servo  parameters  in  use 
for  the  motion  control  of  SHEPHERD.  Therefore,  a  few  test  routines  have  been  implemented  in  the 
file  ‘mot or. c’.  These  functions  are 

driveTestO  to  determine  the  driving  parameters 

steerTestO  to  determine  the  steering  parameters 

stopTestO  to  determine  the  interaction  between  driving  and  steering  for  dig¬ 

its  commanded  to  the  servos  being  zero 

velocityTest  ()  to  obtain  a  relationship  between  digits  commanded  to  the  driving 

motors  and  actual  angle  rates  observed 
circumferenceTestO  to  determine  the  circumference  of  the  wheels 


1.  Steer  Parameters 


For  determining  the  steering  parameters  the  following  method  has  been  impemented  in 
function  ‘steerTest()’  in  file  ‘motor. c’: 
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1.  align  all  wheels  with  hall  sensor 

2.  clear  the  counters 

3.  save  counter  data  in  variable  previous 

4.  rotate  wheels  for  a  certain  number  of  turns  and  stop  time  it  takes  to  rotate  the  wheel 

5.  read  shaft  encoder  ’current’  and  compute  the  counter  difference  to  obtain  the  rate  of  turn 
and  number  of  counts  for  a  turn 

The  source  code  is  implemented  as  function  ‘steerTest()’  in  the  file  ‘motor.c’.  It  should  be 
noted  that  this  test  should  only  be  conducted  for  free  wheels  off  the  ground,  otherwise  the  vehicle 
may  just  wander  around. 

Some  characteristic  data  corresponding  to  a  specific  velocity  commanded  is  shown  in  Ta¬ 
ble  4.1.  It  can  be  seen  from  the  Table  that  when  steering  the  wheel,  this  would  interfere  with  the 
drive  counters  as  well.  The  work  of  Mays/Reid  account  for  this  fact  by  closed  loop  control.  The 
data  was  taken  for  no  load  applied  to  the  wheels  (free  turning  wheels). 


count  per  turn 

Wheel  1 

Wheel  2 

Wheel  3 

Wheel  4 

-92160.2 

-92131.7 

-92160.3 

-92160.1 

counts  per  degree 

-256.00 

-255.92 

-256.00 

-256.00 

time  per  turn  (sec) 

6.97 

6.98 

6.98 

6.98 

drive  count  for  turn 

2048.0 

2047.9 

2048.0 

2047.9 

Table  4.1:  Steering  Wheel  Data  at  Digits  commanded  OxObOO  averaged  over  10  turns. 

Note  when  a  positive  value  is  commanded  to  all  steering  motors  that  the  motion  of  the 
wheels  as  viewed  from  above  is  counterclockwise  and  the  shaft  encoder  readings  are  negative!  From 
the  data,  we  can  derive  a  relationship  between  the  angular  position  of  the  steering  motors  and  the 
encoder  readings 

steering  wheel  1...4  1  degree  =  256  counts 

angle  turned  [radians]  0  =  6.8177  •  IQ-^rad/ count 

Table  4.2:  Conversion  Factor  for  Steering  all  Wheels. 

The  results  given  above  are  in  agreement  with  the  findings  from  Mays/Reid  [1].  With  this 
data  in  mind,  the  angular  velocity  can  be  easily  measured.  All  that  needs  to  be  done  is  to  record 
the  difference  in  steer  encoder  readings  for  an  observation  timeframe  (T=10ms)  and  multiply  by  the 
above  factor  and  divide  by  T. 
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2. 


Drive  Parameters 


What  is  the  goal  to  be  determined  in  this  section  is:  how  does  the  driving  data  commanded 
to  the  drive  servos  (in  the  range  from  -1024  to  +1023)  relate  to  the  actual  driving  speed.  Moreover, 
how  does  driving  interfere  with  the  steering,  is  there  any  leakage  at  all?  In  order  to  determine  this, 
two  functions  are  in  place  for  use  within  the  SRK. 

The  function  MriveTestO’  was  written  in  order  to  determine  how  the  drive  encoder 
readings  relate  to  the  angular  position  of  the  wheel  (if  the  wheel  is  viewed  as  a  clock) .  All  this  function 
does  is  to  record  the  difference  in  shaft  encoder  readings  for  a  given  number  of  turns  completed.  This 
observation  gives  rise  to  the  number  of  counts  per  degree  for  driving  the  wheel.  The  function  does 
not  operate  autonomous  but  rather  requires  user  interaction.  The  user  determines  when  to  start 
and  end  the  observation  period.  This  procedure  was  conducted  several  times  at  different  speeds  - 
although  the  speed  is  not  of  our  concern  at  this  point.  The  results  are  given  in  Table  4.3. 


driving  at  speed  0x0800  (1  turn) 


Table  4.3:  Data  obtained  for  determining  drive  parameters  with  program  ‘driveTestO  \ 

It  can  be  seen  from  the  Table  that  the  number  of  counts  per  degree  for  all  wheels  is  given 
by  approximately  290  counts/degree  except  for  wheel  two  at  the  commanded  speed  of  0x0800. 
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However,  it  is  assumed  that  the  user  simply  failed  in  observing  the  correct  number  of  turns  for  this 
wheel.  Another  test  run  eventually  with  even  more  turns  should  be  conducted.  However,  for  ease  of 
computation  and  in  agreement  to  Mays/Reid  [1],  it  is  expected  that  for  a  given  number  of  encoder 
counts,  all  wheels  will  advance  by  exact  the  same  angle  if  commanded  by  the  same  digit  and  the 
conversion  is  given  by 

driving  wheel  1...4  1  degree  =  290  counts 

angle  driven  [radians]  6  =  6.018376731  •  IQ-^rad/eount 

Table  4.4:  Conversion  Factor  for  Driving  all  Wheels. 


In  a  second  step,  a  function  ‘  velocityTest  ()  ’  was  implemented  in  the  source  file  ‘motor.c’ 
in  order  to  determine  the  driving  speed  as  a  function  of  servo  data  sent  to  the  driving  servos.  The 
inner  workings  of  this  function  are  quite  simple: 

1.  Align  all  wheels,  set  speed  =  500. 

2.  Set  all  motors  to  speed. 

3.  Wait  one  second  to  let  servos  attain  steady  state. 

4.  Observe  the  difference  in  shaft  encoder  readings  for  an  observation  period  of  one  second. 

Store  the  readings  in  main  memory  (starting  at  0x00100000)  at  consecutive  locations. 

5.  Decrease  speed  =  speed  -10. 

6.  If  speed  <  -500  stop,  otherwise  repeat  the  loop  with  step  2. 

7.  Stop  the  test  program. 

Once  the  program  was  done,  the  data  (steering  and  driving  delta  for  every  second)  was 
downloaded  as  an  ASCII  dump  to  the  notebook,  converted  to  decimals  and  further  analyzed  using 
the  MATLAB  function  ‘ velocity. m».  Although  it  was  -  based  on  the  results  from  Mays/Reid 
-  expected  to  obtain  a  nonlinear  relationship  between  the  velocity  (which  is  proportinal  to  the 
difference  in  encoder  readings)  and  the  commanded  digits,  the  results  proved  to  be  quite  different. 

For  free  floating  wheels,  the  drive  encoder  advances  for  a  given  speed  during  the  time  interval 
of  1  sec  are  shown  in  Figure  4.1  and  the  equivalent  steer  encoder  differences  are  shown  in  Figure  4.2. 
To  solidify  the  results,  a  second  experiment,  now  with  the  vehicle  on  the  ground  has  been  conducted. 
The  results  according  to  this  experiment  are  shown  in  Figure  4.3  and  Figure  4.4. 

As  can  be  seen  from  the  graphs,  both  experiments  show  the  same  linear  relationship  for  the 
driving  of  all  wheels  with  just  slightly  changing  parameters  and  in  addition  to  this,  the  interaction 
from  driving  to  steering  for  each  wheel  is  insignificant  and  can  be  neglected.  The  test  was  conducted 
a  total  of  three  times,  two  times  with  the  wheels  on  the  ground  and  the  vehicle  moving  in  a  straight 


22 


Wheel  2.  fit  digits  =  -1 .33028300-002  driveDelta  +  -1 .60266  Wheel  1 ,  fit  digits  =  1 .331 1 8326-002  driveDelta  +  -1 .72896 


velocity  (driveDelta)  [counts/sec]  x  10^  velocity  (driveDelta)  [counts/sec]  x  10* 


Wheel  4,  fit  digits  =  -1 .331 5856e-002  driveDelta  +  0.60989  Wheel  3.  fit  digits  =  1 .3295249e-002  driveDelta  +  -0.S3392 


velocity  (driveDelta)  [counts/sec]  x  10*  velocity  (driveDelta)  (counts/sec]  x  10* 

Figure  4.1:  Commanded  Digits  versus  Encoder  Differences  for  Free  Floating  Wheels. 


Wheel  4 


digits  to  servo 


Wheel  3 


digits  to  servo 


Figure  4.2:  Influence  of  Commanded  Drive  Digits  on  Steering  Wheels.  Plot  shows  Encoder  Differ¬ 
ences  vs.  Commanded  Drive  Digits  for  Steering  Motors  (Steering  Motors  set  to  zero). 
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Wheel  2,  fit  digits  =  -1.3301 071 e-002  driveDelta  +  -1.75192 


velocity  (driveDelta)  [counts/sec]  x  10* 


velocity  (driveDelta)  [counts/sec] 


Wheel  4,  fit  digits  =  -1.3314266a-002  driveDelta  +  0.47206 


velocity  (driveDelta)  (counts/sec)  x  10* 


velocity  (driveDelta)  [counts/sec]  x  10* 


Figure  4.3:  Commanded  Digits  versus  Encoder  Differences  for  Vehicle  on  the  Ground. 


Wheel  4 


digits  to  servo 


Wheels 


digits  to  servo 


Figure  4.4:  Influence  of  Commanded  Drive  Digits  on  Steering  Wheels  for  Vehicle  on  the  Ground. 
Plot  shows  Encoder  Differences  vs.  Commanded  Drive  Digits  for  Steering  Motors  (Steering  Motors 
set  to  zero). 
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line  and  a  third  time  with  the  vehicle  lifted  off  the  ground  and  the  wheels  rotating  free.  Despite 
the  changing  test  conditions,  the  results  were  independent  from  the  way  the  vehicle  was  suspended. 
The  recorded  data  for  each  wheel  was  fitted  in  a  least  square  sense  by  a  polynomial  of  order  1  (a 
straight  line)  and  the  coefficients  are  given  in  Table  4.5  where  the  encoder  difference  driveDelta  is 
given  in  units  of  counts  per  second. 


Wheel  1 

digit  =  0.01331  driveDelta  [count/sec]  -  1.65 

Wheel  2 

digit  =  -0.01330  driveDelta  [count /sec]  -  1.65 

Wheel  3 

digit  =  0.01329  driveDelta  [count /sec]  -  0.30 

Wheel  4 

digit  =  -0.01331  driveDelta  [count /sec]  +0.55 

Table  4.5:  Relationship  between  drive  encoder  difference  and  commanded  servo  drive  speeds. 


It  is  beneficial  to  use  the  relationship  digit=f(driveDelta/sec)  vice  the  inverse  since  for  any 
motion  control  process,  we  are  given  the  desired  speed  (which  is  directly  proportional  to  the  variable 
driveDelta/sec)  and  want  to  obtain  the  required  digit  to  control  the  servos  accordingly.  Using  the 
conversion  factor  given  for  driving  the  wheels  (see  Table  4.4)  and  the  wheel’s  radius  (which  we 
assume  to  be  equal  for  all  wheels  to  be  18.9cm)  we  obtain  the  conversion  from  distance  travelled  to 
count  advances  by 

27r 

1  count  =  — - — •  18.9  cm  =  1.13747  •  10”^  cm 
360  *  290 

1  m  =  87914  counts  (4.1) 

and  we  finally  end  up  with  a  handy  relationship  between  velocity  [cm/sec]  and  digits  commanded 
to  the  servos  (the  digits  are  not  yet  left  justified): 


Wheel  1 

digit  =  11.70  v  [cm/sec]  -  1.65 

Wheel  2 

digit  =  11.69  V  [cm/sec]  -  1.65 

Wheel  3 

digit  =  11.68  v  [cm/sec]  -  0.30 

Wheel  4 

digit  =  11.70  V  [cm/sec]  +0.55 

Table  4.6:  Relationship  between  Velocity  [cm/sec]  and  Commanded  Servo  Digit  (needs  further  be 
multiplied  by  16  to  justify  left). 

After  multiplying  the  above  data  by  16  in  order  to  shift  it  digital  wise  one  nibble  to  the  left, 
we  obtain 

Table  4.7  yields  the  values  that  can  be  directly  sent  to  the  driving  servos.  They  will  already 
yield  the  left-justified  data  sent  to  the  analog  output  board.  Recall  that  only  the  upper  12  bit 
determine  the  final  servo  speed.  Hence,  when  driving  the  wheels,  we  encounter  a  discretization  error 
introduced  by  converting  the  double  valued  velocity  to  12  bit! 
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B. 


LINEAR  MOTION  PROFILE 


In  order  to  test  the  sampling  results  obtained  from  both,  the  shaft  encoder  and  the  IMU, 
a  simple  linear  motion  profile  was  implemented  in  the  SRK.  The  profile  is  implemented  as  routine 
'linearMotionlO  ’  m  the  source  file  ‘motor. c’  and  is  shown  in  Figure  4.5.  As  it  turned  out  later, 
this  profile  was  not  suitable  to  obtain  reliable  data.  Hence,  a  second  profile  was  implemented  as 
routine  ‘linearMotion2()  ’  and  the  vehicle’s  principle  behavior  is  depicted  in  Figure  4.6.  While 
the  vehicle  would  travel  a  distance  of  4  m  in  forward  direction  and  return  to  its  start  position 
upon  execution  of  ‘linearMotionlO  >,  it  would  travel  for  5/6  of  a  meter  forward  and  stop  for 
‘linearMotion2()  ».  However,  the  vehicles  maximum  acceleration  for  the  former  motion  would  be 
2  cm/sec^  while  for  the  latter,  the  vehicle  would  speed  up  to  1  m/sec^  which  is  quite  high! 

In  the  following,  the  results  for  the  shaft  encoders  for  both  motion  profiles  will  be  discussed 
utilizing  the  motion  control  procedure  as  outlined  in  Chapter  II  on  page  12.  The  analyzing  MATLAB 
routine  ‘shaft.m’  is  for  completeness  given  in  Appendix  B.5  on  page  65. 


1.  Linear  Motion  Profile  #1 

This  motion  segment  lasts  for  a  total  of  70  seconds,  after  which  the  vehicle  is  expected  to 
have  returned  to  its  start  position.  The  stop  during  the  period  SOsec  <t  <  40sec  is  utilized  to  mark 
the  turning  position  for  the  vehicle. 

Clearly,  as  Figure  4.8  reveals,  the  driving  angles  are  off  by  up  to  10  degrees  upon  completion 
of  the  motion  program.  On  the  floor,  a  lateral  deviation  of  approximately  35  cm  has  been  observed. 
The  longitudinal  distances  traveled  came  out  to  be  395  cm  for  the  forward  leg  and  401  cm  for  the 
reverse  leg. 

Despite  the  fact  that  the  steering  motors  are  set  to  zero,  there  remains  interaction  between 
driving  and  steering.  It  needs  to  be  determined  whether  or  not  this  relates  to  badly  adjusted  (offset) 
servo  motors  or  indeed  driving  interaction.  In  any  case,  it  is  quite  evident  that  feedback  is  required 
to  provide  the  desired  accuracy  for  straight  motion.  The  aspects  of  feedback  are  not  discussed  in 


Wheel  1 

digit  =  187.20  v  [cm/sec]  -  26.4 

Wheel  2 

digit  =  187.04  v  [cm/sec]  -  26.4 

Wheel  3 

digit  =  186.88  v  [cm/sec]  -  4.8 

Wheel  4 

digit  =  187.20  v  [cm/sec]  +  8,8 

Table  4.7:  Relationship  between  Velocity  [cm/sec]  and  Commanded  Servo  Digit. 
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linearMotion1() 
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this  thesis.  However,  Mays/Reid  [1]  provide  a  brief  discussion  about  this  topic. 


2-  Linear  Motion  Profile  #2 

In  order  to  serve  the  IMU  analysis  better,  a  linear  motion  profile  was  needed  which  provided 
a  greater  acceleration  for  the  vegicle.  Thus,  the  linear  motion  program  ^ linearMotion2()  ^  has  been 
implemented  in  the  file  ^  motor .  c  \  This  motion  program  drives  the  vehicle  over  a  distance  of  about 
83  cm  (5/6  m)  within  4  sec.  As  was  for  the  motion  profile  #1,  the  vehicle  follows  closely  the 
determined  path. 

Considering  the  fact  that  no  feedback  has  been  implemented  in  the  motion  control  programs, 
it  can  be  concluded  that  the  shaft  encoder  readings  provide  sufficient  accuracy  for  determining  the 
planar  motion  for  SHEPHERD  under  the  condition  that  no  slip  occurs. 


C.  UNCERTAINTIES  IN  MOTION  CONTROL 

It  is  quite  obvious  that  the  accuracy  of  the  motion  control  part  and  the  position  determina¬ 
tion  depends  on  several  parameters  that  may  vary  over  time  or  that  were  determined  too  inaccurate. 
The  main  reasons  for  inaccurate  motion  control  and  position  determination  derived  from  the  shaft 
encoder  readings  are 

1.  Inaccurate  sensor  parameters  relating  to  the  angular  position  of  each  motor. 

2.  Wheel  radius  not  measured  correctly  or  radius  changing  over  time  due  to  wear  or  changing 
tire  pressure. 

3.  Data  reduction  for  velocity  from  double  valued  data  type  to  12  bit  that  are  being  sent  to 
the  servos. 

All  these  factors  will  eventually  degrade  the  performance  of  the  implemented  routines.  Hence,  there 
will  be  ample  space  for  improvement  for  future  work. 
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V. 


INERTIAL  MEASUREMENT  UNIT 


This  chapter  describes  the  framework  that  was  implemented  on  SHEPHERD  in  an  attempt 
to  obtain  reliable  velocity  and  position  data  based  on  inertial  measurements.  All  source  code  as  it 
pertains  to  the  implementation  of  the  Inertial  Measurement  Unit  (IMU)  is  provided  in  the  source 
file  fimu.c’  and  listed  in  Appendix  C.l  starting  at  page  67. 

Figure  5.1  shows  the  vehicle’s  basic  appearance  with  the  four  wheels  at  the  corners  labelled 
1  to  4  and  the  motion  sensor  with  its  three  corners  marked  by  a  solid  dot  which  span  the  xy-plane 
in  the  body  frame  {B}  mounted  on  its  steel  plate.  The  solid  dots  on  the  sensor’s  casing  are  just  to 
relate  the  upside  down  orientation  to  the  general  appearance  as  given  by  Figure  5.2. 


Figure  5.1:  Configuration  for  Shepherd  Rotary  Vehicle 

Due  to  the  particular  design  of  the  SHEPHERD  Rotary  Vehicle,  the  vertical  axes  of  each 
wheel  are  exactly  located  on  the  corners  of  a  square  of  dimension  0.8  x  0.8  m.  The  sensor  is  mounted 
upside  down  below  the  supporting  steel  plate  at  the  location  indicated  in  Figure  5.1, 
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A.  INERTIAL  SENSOR 

For  this  project,  a  four  degree  of  freedom  inertial  sensor  cluster  (Solid-State  Motion  Sensor, 
Type  MotionPalc)  from  SYSTRON  Donner,  Concord  California  [8]  is  being  used.  It  provides  three 
outputs  for  linear  motion  measured  with  servo  accelerators  {a^^ay,  ay)  and  one  output  for  measuring 
rotational  motion  about  the  z-axis  (w^).  This  data  comprises  a  cartesian  coordinate  system  which 
is  shown  in  Figure  5.2.  The  dots  in  the  three  corners  shall  help  identify  the  attitude  of  the  sensor 
as  shown  in  Figure  5.1. 


Figure  5.2:  Axis  orientation  for  MotionPalc  Sensor 


The  MotionPak  is  customized  by  the  manufacturer  for  the  anticipated  dynamic  range.  Ta¬ 
ble  5.1  shows  most  of  the  specifications  as  they  apply  to  the  model  in  use. 


x-axis 

y-axis 

z-axis 

dx 

dz 

Range 

±29 

±2g 

±2g 

±50°/sec 

Scale  factor 

S.74SV/g 

3.752V/.9 

Z.744Vlg 

49.881mVy  (deg  /  sec) 

Stationary  output 

0.0  V 

0.0  V 

-f3.75  V 

0  V 

Bandwidth 

869  Hz 

925  Hz 

869  Hz 

75  Hz 

Noise  (lO-lOOHz) 

1.8  mVjtMS 

1.8  mVijA/5 

2,0  mV  RMS 

3.9 

Table  5.1:  Operating  specifications  for  MotionPak  Model  No.  MP-G-CQBBB-100,  Serial  No.  0329 
(after  Reference  [9]) 


As  was  already  shown  by  Figure  2.4  on  page  8,  the  analog  data  provided  by  the  MotionPak 
IMU  is  converted  into  digital  data  by  an  A/D-Board  interfacing  to  the  VMEBus.  The  converted 
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digital  data  is  transferred  from  the  A/D-Board  to  the  68040  processor  on  the  TUARUS  board  via 
the  VMEBus.  Figure  5.3  shows  how  the  four  analog  channels  from  the  MotionPak  IMU  are  actually 
routed  through  the  A/D-Board  to  the  CPU. 


Figure  5.3:  IMU  Hardware  Integration 


B.  A/D  CONVERSION  SCHEME 

The  IMU  provides  continuous  analog  data  to  channels  1  to  4  of  the  A/D-Board  VME9325 
[10],  With  every  10  ms  timer  interrupt,  a  block  conversion  on  the  AD-Board  is  triggered  via  software 
command  issued  by  the  interrupt  handling  routine  from  the  10  ms  timer.  The  AD-Board  is  configured 
to  multiplex  the  four  input  channels  every  50  //  sec  for  a  total  of  200  samples.  Thus,  in  a  consecutive 
order,  each  of  the  four  channels  are  sampled  at  a  sampling  rate  of  /5=5000  Hz  and  the  digital  data 
is  stored  sequentially  in  the  A/D-Boards  dual-port  RAM.  Once  the  block  conversion  is  complete, 
the  A/D-Board  will  issue  an  interrupt  (see  Appendix  D.4  on  page  93  for  the  exact  interrupt  level 
in  use)  to  68040  where  the  corresponding  interrupt  handler  routine  analyzeVME9325()  preprocesses 
(filters)  the  block  data  and  stores  it  as  the  most  recent  data  in  the  global  variables 


Clx 

ay 


imuAX 

imuAY 

imuAZ 

imuOmegaZ 
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which  will  thus  be  available  for  the  next  motion  control  cycle  to  update  the  actual  vehicle  motion. 
The  board’s  status  can  be  observed  by  means  of  LED  indicator  lights  at  the  boards  front  panel; 


Green  LED 

Red  LED 

Status 

off 

on 

Board  is  not  initialized 

on 

on 

Board  undergoes  initialization 

off 

off 

Board  is  initialized  but  inactive 

on 

off 

Board  is  performing  A/D  block  conversions 

Table  5.2:  Status  indicator  lights  for  A/D-Board 


At  present,  the  data  is  merely  downloaded  via  the  TAURUSBug  ’duO’  option  (see  Ap¬ 
pendix  D.3)  through  the  CONSOLE  port  to  the  Laptop  and  from  there  to  the  UNDC  System,  where 
the  data  was  further  analyzed  using  MATLAB.  However,  for  the  future,  the  sampled  data  would  be 
directly  processed  by  the  68040  processor  as  outlined  above. 

One  might  ask,  why  was  the  odd  sampling  frequency  =  5000  Hz  is  being  used  instead  of 
a  more  intuitive  10  kHz.  A  look  at  the  timing  diagram  Figure  5.4,  reveals  that  the  time  A  between 
the  last  block  conversion  {u>z  in  block  50)  and  the  start  of  the  next  motion  control  cycle  is  governed 
by  the  sampling  frequency:  for  continuous  sampling  (e.g.,  increased  block  number  to  transfer),  the 
larger  the  smaller  will  A  be.  However,  there  is  a  constraint  on  the  minimum  length  of  A  due 
to  the  fact  that  the  sampling  block  data  must  be  transferred  to  the  TAURUS  main  memory.  This 
transfer  must  be  done  before  the  next  motion  control  cylce  is  issued  by  the  10  ms  timer  interrupt. 
This  rule  must  be  closely  followed,  otherwise  a  loss  of  sampling  data  might  occur. 


Timer  Interrupt  k 


Timer  Interrupt  k+1 


4 


t  [ms] 


Block  50  I  Block  1 
^ _  ^ 

“V  u>2  fa®  o-y 

I  t  t  l!t  t  t  t 


t  [ms] 


Figure  5.4:  Timing  Diagram  for  A/D-Board 

The  A/D-Board  maps  a  preset  input  span  of  A  =  20  V  for  a  differential  input  range  of  ± 
10  V  into  n=12  bit  bipolar  two’s  complement  data  left  justified  in  a  16  bit  word.  The  value  of -2048 
relates  to  an  analog  input  equvalent  of -10  V  <  Xanaiog  <  -9.99512  V.  Likewise,  the  digital  output 
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of  2048  relates  to  0  V  <  Xanaiog  <  0.00488  V.  The  stepsize  is  given  by  <5  =  ^  =  4.88  mV.  To 

make  use  of  the  maximum  range  available,  the  board  provides  a  variable  gain  to  amplify  the  input 
signal  by  factors  G=l,  G=2,  G=4,  or  G=8.  Moreover,  we  need  to  scale  the  data  by  the  appropriate 
scaling  factors  S  for  each  channel  which  are  given  in  Table  5.1.  Thus,  for  a  given  channel  with  gain 
G  and  scaling  S,  we  obtain  the  analog  equivalent  of  the  data  by  shifting  the  digital  value  x digital  by 
4  bit  to  the  right  (which  is  equivalent  to  a  division  by  16)  and  than  re-scale  it  according  to: 

A 

Xanaiog  ”  2^  G  S  ~ 

Using  the  scaling  factors  given  in  Table  5.1  we  end  up  with  the  units  of  [g]  for  ax,  ay,  and  az  and 
[degrees/sec]  for  Uz-  Expressing  the  linear  acceleration  a  in  terms  of  the  gravitational  acceleration 
g  rather  than  in  Sl-units  of  [m/sec^]  turns  out  to  be  beneficial  if  we  need  to  find  the  Euler  angles 
and  a  suitable  representation  for  it  in  the  reference  frame  {R}. 


C.  SCHEME  FOR  DATA  ANALYSIS 

Accelerometers  sense  the  sum  of  the  gravitational  acceleration  Ug  and  the  linear  acceleration 
a  which  is  due  to  an  external  force  applied  to  the  body  in  the  body  frame  {B} 


®a  +  ®g 

(5.1) 

which  relates  to  the  reference  frame  {R}  as 

= 

(5.2) 

In  both  frames,  g  is  the  acceleration  of  gravity  derived  from  Keplerian  physics  for  two  body  motion 
theory  between  the  Earth  and  a  body.  Usually,  g  is  a  function  of  the  distance  r  between  the  center 
of  masses  of  the  two  bodies  and  can  be  computed  with 

GM 


with  the  constants  G  and  M  as  described  in  Appendix  A.  For  a  body  at  the  Earth’s  surface, 
g  ^  9.81  m/sec^  and  usually,  the  variation  in  height  for  small  changes  can  be  neglected.  Therefore 
we  will  not  concern  ourselves  with  a  variable  g  and  assume  that  g  =  9.81  mfsec^. 

In  the  following,  we  will  devise  a  scheme  to  eliminate  the  undesired  gravity  components  in 
our  measurement  data.  Therefore,  we  will  have  to  focus  on  the  stationary  vehicle  first,  that  is,  the 
only  acceleration  acting  on  the  vehicle  in  frame  {B}  will  be  the  Earth  gravity.  Moreover,  we  know 
that  in  the  reference  frame  {R},  the  acceleration  due  to  gravity  has  only  a  -hz-component  whereas 
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in  {B}  we  would  usually  encounter  gravitational  components  in  each  of  the  principal  axes  unless  the 
sensor  is  perfectly  aligned  with  frame  {R}: 


subject  to  the  constraint  that  g  =  y^giT^g^T^.  To  express  frame  {B}  in  terms  of  frame  {R} 
we  make  use  of  the  rotation  matrix  as  outlined  in  the  previous  sections  and  given  by  Equation  3.2: 


We  therefore  do  need  to  get  the  Euler  Angles  (roll,  pitch,  and  yaw)  as  defined  on  page  17.  We  make 

us  of  the  fact  that  the  acceleration  of  a  stationary  sensor  as  measured  in  {R}  should  only  display 
the  gravitation: 


Solving  for  ®am  yields 


Sm  =  0  =  *'Rz(»/')  '^RyC^)  ^Rx(0)°a„ 


=  -R-l(^)  «R-l(^)  aR-l(0) 


We  recall  the  identity  given  in  Equation  3.1  on  page  16  and  rewrite  the  above  equation  in  terms  of 
the  transpose  of  each  rotation  matrix: 


For  any  measurement  vector  and  the  related  vector  “S  in  frame  {R},  Equation  5.3  together 
with  the  definitions  for  the  rotation  matrices  Equation  3.3,  Equation  3.4  and  Equation  3.5  given  on 
page  17  provides  us  with  a  system  of  three  equations  from  which  we  can  determine  the  Euler  Angles. 
In  particular,  we  are  easily  able  to  determine  the  Euler  angles  as  a  function  of  the  measurement 


=  -g  sin{<p) 
aym  =  9  sin{6)  cos{4>) 

o^zm  =  g  COs{d)  COs(4>) 


(5.4) 

(5.5) 

(5.6) 


We  recognize  that  for  the  stationary  data,  the  acceleration  measured  in  {B}  does  not  depend  on  the 
yaw  angle  ip  which  is  directly  related  to  the  heading  of  the  vehicle  (in  order  to  obtain  the  heading,  we. 
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of  course,  would  need  to  have  a  compass  at  hand).  Solving  the  above  system  for  the  two  remaining 
Euler  angles  yields  the  following  equations: 


or  alternatively  for  6 


(5.7) 

(5.8) 

(5.9) 


We  see  that  the  last  two  equations  both  yield  a  solution  for  0,  Depending  on  the  accuracy  of  our 
measurements  and  the  accuracy  of  the  desired  math  functions  we  have  implemented  so  far,  we  may 
prefer  the  one  to  the  other.  Since  the  Sensor’s  output  data  is  already  scaled  with  respect  to  g,  the 
Earth’s  gravity  (see  Table  5.1),  we  may  prefer  the  former  and  discard  Equation  5.9.  This  is  reflected 
in  the  MATLAB  listing  for  ‘getdata.m’  where  the  data  is  arranged  accordingly. 

Based  on  the  theory  pertaining  to  the  inertial  measurement  sensor  as  outlined  above,  the 
following  scheme  to  obtain  the  position  data  for  the  vehicle  is  proposed: 


1.  Sample  stationary  data  (as  is  usually  the  case  if  one  starts  up  the  vehicle)  in  frame  {B}  for 
a  certain  period  of  time. 

2.  Filter  the  data  with  an  appropriate  lowpass  filter. 

3.  Compute  the  Euler  angles  6  and  0. 

4.  Transform  the  data  from  frame  {B}  to  frame  {R}  using  the  rotation  matrices  given  by 
Equation  3.2,  use  arbitrary  yaw  angle 

5.  Subtract  the  acceleration  due  to  gravity  acting  on  the  vehicle  to  obtain  the  sole  acceleration 
due  to  a  specific  force  given  in  frame  {R}. 

6.  Integrate  the  data  in  a  suitable  way  to  find  the  velocity  and  position  vector  of  the  vehicle. 


D.  INTEGRATION  TOOLS 

In  our  analysis  of  the  inertial  measurement  sensor,  we  will  have  to  integrate  the  data  in  order 
to  arrive  at  the  velocity  vector.  There  are  many  integration  methods  available  for  integrating  discrete 
data.  For  equispaced,  discrete  data,  most  of  the  more  commonly  known  integration  formulas  such 
as  the  Trapezoidal  rule,  Simpson’s  Rule,  ...  are  based  on  the  Newton-Cotes  Integration  Formulas 
([11], [12]).  Given  a  set  of  values  S{xi)  for  equispaced  Xi  —  a  +  ihM  i  =  {) . .  .n  with  h  =  the 
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integral  of  / (x)  on  the  interval  [a,  6]  can  be  approximated  by 

f  f(x)  dx=  f  Pn{x)  dx 
J  a  J  a 

where  P„(a;)  is  the  Lagrangian  polynomial  that  passes  through  all  the  points  Xj  and  the  interval 
[a,  6]  is  covered  by  the  (n+1)  equidistant  points  x,.  P„(x)  is  given  by 

n 

p„(x)  =  Y^f{xi)  ai 
i=0 

where  is  given  by 

-afe) 

If  we  let  X  =  a-\-  hs  the  above  integral  for  (x)  reduces  to  a  simple  sum 

f 


P„(x)  dx  =  h^f{xi)  ai  =  ^  'y  (Tifixi) 

7^0  S 


(5.10) 


The  values  for  ns  and  cjj  can  be  computed  given  the  above  relations.  However,  we  will  not  concern 
ourselves  with  this  issue  and  state  the  results  for  the  first  few  parameters: 


n 

ns 

Commonly  known  rule 

1 

2 

1  1 

Tapezoidal 

2 

6 

14  1 

Simpson’s  1/3 

3 

8 

13  3  1 

Simpson’s  3/8 

4 

90 

7  32  12  32  7 

5 

'  288 

19  75  50  50  75  19 

6 

840 

41  216  27  272  27  216  41 

Table  5.3:  Newton-Cotes  Formula  Parameters 


Some  of  these  formulas  are  being  implemented  in  the  function  ‘  integral  .m’  on  page  page  65 
and  used  for  integrating  the  acceleration  data.  The  analysis  in  the  following  sections  will  discuss 
which  formula  shall  be  preferred  to  the  others. 


E.  DATA  FILTERING  AND  COMPUTATION  OF  POSITION  VECTOR 

Several  recordings  for  stationary  data  have  been  taken.  In  the  process  of  obtaining  the 
position  vector  for  the  vehicle  we  would  expect  that  starting,  say  from  an  initial  position  (0, 0, 0){fl}, 
this  should  not  vary  much  as  time  passes  by. 

Initially,  the  sampling  scheme  was  such  that  each  channel  of  the  IMU  was  sampled  at  a 
sampling  rate  of  100  Hz  with  every  10  ms  timer  interval.  Later  on,  this  has  been  changed  to  a 
sampling  rate  of  5000  Hz  as  shown  in  the  timing  diagram  Figure  5.4  on  page  34. 


38 


1. 


Stationary  Data  Analysis 


The  data  collected  for  the  stationary  data  analysis  in  this  subsection  has  been  sampled 
prior  to  changing  the  sampling  frequency  from  100  Hz  to  5000  Hz.  Thus,  this  is  reflected  in  the 
data  presented  in  this  subsection.  In  addition,  the  IMU  at  this  stage  was  not  yet  mounted  to  the 
vehicle  and  the  orientation  of  the  axes  was  such  that  the  sensors  z-axis  pointed  up  instead  of  down 
as  shown  in  Figure  5.1.  Figures  5.5  to  5.10  show  typical  results  obtained.  They  show  data  recorded 
and  processed  for  a  stationary  vehicle  with  file  4mu.m’  (see  Appendix  B.l  on  page  59).  The  data 
was  recorded  on  the  fifth  floor  of  Spanagel  Hall  with  the  sensor  titled  by  a  significant  amount  which 
was  not  further  specified. 

As  can  be  seen  from  Figure  5.6,  the  linear  components  (ux,  Uy,  and  a^)  contain  distinct 
sinusoidal  components  at  /  =  20 Hz  and  /  =  40Hz.  The  origin  of  this  behavior  still  needs  further 
examination.  However,  it  seems  not  to  be  related  to  the  block  sampling  interval  of  T=10  ms,  rather 
than  to  vibrations  inherent  in  the  building.  These  sinusoidal  components  can  not  be  beneficial  to 
the  performance  of  our  compuations.  Therefore,  we  have  to  eliminate  the  residues  by  some  suitable 
filtering  technique. 

In  the  time  domain  (Figure  5.5),  we  see  the  effect  due  to  the  A/D  sampling  process:  the 
sampled  data  obtained  through  the  A/D  Board  truly  displays  the  characteristics  for  discrete-time 
signals.  Moreover,  since  the  sensor  was  titled,  the  data  will  reflect  the  values  according  to  this 
orientation  relative  to  frame  {R}.  Thus,  the  next  step  involves  computation  of  the  Euler  angles  and 
transforming  the  data  into  frame  {R}  using  the  results  obtained  in  Equation  3.2.  Now,  follwoing  the 
transformation  the  data  for  ax  and  ay  should  ideally  go  to  zero  (at  least  in  the  mean).  The  result 
is  shown  in  Figure  5.7  with  its  Fourier  spectrum  given  by  Figure  5.8. 

In  fact,  the  acceleration  for  ax  and  ay  is  almost  zero  whereas  the  acceleration  for  az  is  almost 
— 1.0  g  (the  DC  component  is  not  shown  in  the  frequency  spectrum.  The  negative  sign  for  this  data 
set  is  due  to  the  fact  that  the  sensor’s  z-axis  pointed  down.  The  final  step  is  to  obtain  the  velocity 
and  the  position  by  integrating  the  acceleration  once  or  twice,  respectively.  The  velocity  is  shown  in 
Figure  5.9.  As  can  be  seen  from  the  plot,  the  velocity  in  x-  and  z-direction  pretty  much  approaches 
steady-state  after  about  3  sec  of  recording  whereas  the  velocity  in  y-direction  approaches  steady  state 
after  about  10  seconds  (eventually,  a  longer  recording  needs  to  be  taken  to  verify  this  statement).  As 
for  the  position  vector,  which  is  shown  in  Figure  5.10,  we  see  that  during  the  first  second  the  error  is 
small  and  the  position  remains  pretty  much  zero.  However,  as  the  velocity  assumes  its  steady  state, 
the  position  displays  a  linear  behavior.  Therefore,  based  on  the  stationary  analysis,  it  is  advisable 
to  update  (reset)  the  navigation  solution  based  on  the  IMU  at  least  every  second.  Even  better,  if 
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Figure  5.5:  Time  domain  behavior  for  linear  acceleration  and  angular  velocity  for  the  stationary  and 
tilted  IMU  as  measured  by  the  A/D-Board  (normalized  to  units  [g])  in  frame  {S}. 


Spectrum  for  linear  acceleration  without  DC-component 
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Figure  5.6:  Fourier  spectrum  for  linear  acceleration  and  angular  velocity  for  the  stationary  and  tilted 
IMU  as  measured  by  the  A/D-Board  (normalized  to  units  [g])  in  frame  {S}. 
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Figure  5.7;  Time  domain  behavior  for  linear  acceleration  and  angular  velocity  for  the  stationary  and 
tilted  IMU  as  measured  by  the  A/D-Board  (normalized  to  units  [g])  in  the  reference  frame  {R}. 
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Figure  5.8:  Fourier  spectrum  for  linear  acceleration  and  angular  velocity  for  the  stationary  and  tilted 
IMU  as  measured  by  the  A/D-Board  (normalized  to  units  [g])  in  the  reference  frame  {R}. 


the  Euler  angles  which  represent  the  attitude  of  the  vehicle  could  be  determined  continuously  and 
in  accordance  to  the  updated  Euler  angles,  new  rotation  matrices  would  have  to  be  determined  on 
a  regular  basis. 


2.  Non-stationary  Data  Analysis  with  Profile  #1 

In  the  sequel,  we  will  analyze  data  sampled  at  a  sampling  frequency  of  =  5000  kHz 
according  to  the  timing  diagram  depicted  in  Figure  5.4  from  an  IMU  that  is  mounted  on  SHEPHERD 
as  shown  m  Figure  5.1.  First,  in  order  to  correlate  the  sampled  data  to  the  actual  motion  of 
the  sensor/vehicle,  the  same  linear  test  motion  profile  as  introduced  in  Chapter  IV  and  shown  in 
Figure  4.5  on  page  27  was  being  utilized.  Due  to  the  vast  amount  of  data  that  had  to  be  analyzed 
(a  recording  for  70  sec  at  a  sampling  frequency  of  5000  Hz  on  four  IMU  channels  comprised  a  mere 
2.8  MByte!)  the  analysis  was  performed  on  segments  of  data  in  order  not  to  exploit  the  limits  of 
computational  power.  In  particular,  to  enhance  the  performance  of  the  built  in  MATLAB  Fourier 
transform  function,  segments  contained  65536  samples,  which  is  a  power  of  two  (2^®). 

Figure  5.11  depicts  the  linear  acceleration  as  determined  by  the  IMU.  Despite  the  fact  that 
the  linear  motion  profile  was  only  along  the  x-axis  of  the  vehicle,  the  sensor  seemed  not  to  distinguish 
between  the  channels.  All  three  components  display  some  sort  of  noise  and  the  signals  do  not  at  all 
seem  to  be  related  to  the  actual  motion  profile. 

The  detailed  analysis  of  the  a^^-channel  is  given  in  Figure  5.12  and  5.13  for  the  time  frame 
0  <  t  <  13sec.  Figure  5.12  shows  that  the  original  data  is  distorted  throughout  the  entire  frequency 
range.  Moreover,  the  time  signal  does  not  display  the  expected  behavior  according  to  the  true  motion 
profile.  Instead,  the  oscillations  increase  in  amplitude  as  time  advances.  To  reduce  the  noise,  an 
elliptic  filter  has  been  used  to  attenuate  the  noise  in  the  stopband.  The  software  filter,  implemented 
using  MATLAB  s  built  in  signal  processing  functions,  had  the  following  specifications: 

1.  Passband  from  0 ...  20  Hz  with  max.  attenuation  of  0.1  dB 

2.  Stopband  from  50...  Hz  with  min.  attenuation  of  80  dB 

Other  filters  such  as  Chebychev  and  Butterworth  filters  were  also  being  tested.  None  of 
these  filter  types  showed  a  significant  improvement  of  the  data.  The  only  advantage  Butterworth  or 
Chebychev  filters  have  compared  to  Elliptic  filters  is  a  better  pheise  linearity  in  the  passband.  On  the 
other  hand,  and  most  important  for  an  implementation  where  computation  time  is  scarce.  Elliptic 
filters  are  most  efficient  since  they  yield  the  smallest-order  filter  for  a  given  set  of  specifications  [14]. 
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Figure  5.13:  Analysis  of  linear  acceleration  ax  after  data  has  been  filtered  by  a  6th  order  elliptic 
filter  with  passband  edge  at  20  Hz  and  Stopband  edge  at  50  Hz. 
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The  results,  as  depicted  in  Figure  5.13  do  not  look  too  promising.  Althought  the  filter 
achieved  to  smooth  the  data  and  reduce  the  noise,  it  could  not  ensure  that  the  acceleration  would 
show  any  transition  at  t=10sec.  Recall  that  according  to  the  true  profile,  the  acceleration  should 
be  zero  starting  with  t=10sec.  The  only  reason  that  can  be  attributed  to  this  fatal  behavior  is 
the  dynamic  input  range  of  the  A/D-Board;  operating  the  accelerometer  at  a  maximum  linear 
acceleration  of  Ux  =  O.Q2m/se(?  (which  is  only  0.002  g)  we  utilize  only  a  voltage  span  from  -7.6 
mV  to  -F7.6  mV  that  is  fed  into  the  A/D-Board.  Even  if  the  maximum  gain  of  8  is  used  to  amplify 
this  signal,  the  amplitude  would  never  exceed  ss  62  mV  which  comprises  a  mere  four  digits  in  the 
digital  output  range. 


3.  Non-stationary  Data  Analysis  with  Profile  #2 

It  was  anticipated  that,  for  the  second  motion  profile  as  shown  in  Figure  4.6,  results  for  the 
measured  acceleration  would  improve.  The  maximum  acceleration  was  set  to  be  1.0  m/sec^  with 
the  maximum  velocity  reached  by  the  vehicle  to  be  «  0.5  m/s.  The  sampled  data  for  all  three  linear 
acceleration  channels  is  shown  in  Figure  5.14.  The  plot  reveals  strong  interaction  between  all  three 
channels.  One  goal  would  be  to  get  rid  of  these  interferences  by  means  of  a  suitable  filter  technique. 
For  the  time  being,  we  focus  on  the  (Xs-channel.  The  time  and  frequency  behavior  for  the  x-channel 
is  depicted  in  Figure  5.15.  Strong  harmonic  components  influence  the  overall  performance  and  a 
similarity  to  the  actual  motion  can  not  be  found. 

Upon  filtering  with  an  eUiptic  filter  of  order  6,  the  recorded  data  can  somewhat  be  related 
to  the  true  motion.  However,  since  the  sharp  edges  in  the  ideal  acceleration  profile  (Figure  4.6) 
result  in  high  firequency  components  of  the  signal,  these  edges  can  not  be  recognized  by  the  IMU 
(the  cutoff  frequency  for  the  linear  accelerometers  is  around  900  Hz,  see  Table  5.1.  Nonetheless,  the 
questions  remains:  would  this  be  suffice  to  compute  the  velocity?  We  refer  to  Figure  5.16  and  see 
that  the  velocity  does  in  principle  follow  the  curve  depicted  by  the  ideal  motion  profile  Figure  4.6. 
As  soon  as  the  recognizable  motion  kicks  in,  the  velocity  seems  to  be  distorted  by  an  offset  in  the 
acceleration  data  (rather  than  assuming  a=0  on  the  interval  t  €  [2,3]  sec). 


Non-stationary  Data  Analysis  with  Profile  #3 


To  get  rid  of  the  lowpass  constraint,  a  third  motion  profile  has  been  developed.  The  profile 
is  shown  in  Figure  5.17. 
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Linear  acceleration 


Figure  5.14:  Linear  Acceleration  and  angular  velocity  Uz  relative  to  frame  {R}  measured  by  the 
IMU  for  Linear  Motion  Profile  #2. 


Acceleration  ax  in  frame  R,  mean  is  -0.0069787 


1000 


Figure  5.15:  Analysis  of  linear  acceleration  ax  as  measured  by  the  IMU. 


Figure  5.16:  Analysis  of  linear  acceleration  ax  after  data  has  been  filtered  by  a  6th  order 
falter  with  passband  edge  at  20  Hz  and  Stopband  edge  at  50  Hz. 


elliptic 


Position  [m]  Veiocity  (m/secj  Acceleration  [m/sec^2] 


Figure  5.17:  Linear  Motion  Profile  #3. 
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Clearly,  this  motion  should  only  contain  low  frequency  components.  As  was  the  case  for  the 
other  two  motion  profiles,  the  IMU  senses  noise  in  all  three  channels  even  though  the  motion  takes 
place  only  in  the  sensors  x-direction. 


F.  SUMMARY 

Based  on  the  results  obtained  from  the  linear  motion  profiles  ..  ^3  the  following  con¬ 
clusions  for  the  implementation  of  the  inertial  measurement  unit  can  be  drawn:  First,  the  IMU  data 
sampled  off  the  IMU  needs  to  fit  appropriately  in  the  A/D-Boards  input  range.  As  a  crude  rule  of 
thumb  based  on  the  observations  made  in  this  Chapter,  the  time  average  of  the  acceleration  signals 
to  be  A/D-converted  (this  may  include  any  additional  gain)  should  be  at  least  1/10  th  of  the  max. 
allowable  input  amplitude  of  the  A/D-Board  (e.g.,  at  present,  the  max.  input  is  ±  10  V,  the  input 
signal  should  be  at  least  1  V  in  magnitude).  A  more  detailed  analysis  is  required  in  this  respect. 
Probably  the  most  effective  solution  would  be  to  utilize  MotionPak  Accelerometers  (QFA7000)  with 
current  output  rather  than  voltage  output.  In  this  case,  the  output  could  be  scaled  by  the  user 
to  especially  lower  ‘g’  limits  by  means  of  variable  scaling  resistors  (see  [13]  for  more  information). 
Probably  the  most  significant  shortfall  in  the  design  of  the  vehicle  was  determined  to  be  the  variable 
suspension  of  the  vehicle’s  wheels.  Whenever  the  vehicle  accelerates  by  a  significant  amount,  the 
vehicle’s  steel  platform  may  tilt.  This  change  of  attitude  will  be  recognized  by  the  IMU  but  can  not 
be  attributed  to  a  change  of  the  vehicle’s  main  body  attitude  and  thus  to  a  change  of  position  in 
3D  space. 
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vx[tiVs]  axlgl  _  AX[g]  AX[g] 


Acceleration  ax  in  frame  {R).  mean  is  0.009618 

1 

0.5 


-0.5 
-1 

0  1  2  3  4  5  6  7 


FFT  for  ax  [g],  mean  is  0.01381  [g],  fs=5000  Hz 


i^igure  5.18:  Analysis  of  linear  acceleration  ax  as  measured  by  the  IMU. 


Acceleration  ax  in  frame  {R}  after  eliiptic  filtering 


Figure  5.19:  Analysis  after  Elliptic  Filtering  (6th  order  filter)  with  passband  edge  at  20  Hz  and 
Stopband  edge  at  50  Hz. 
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VI. 


SENSOR  FUSION 


Having  developed  the  two  independent  navigation  components  in  the  previous  Chapters,  it 
was  anticipated  to  fuse  the  data  provided  by  both  systems  to  further  improve  the  accuracy  of  the 
navigation  system.  However,  since  the  performance  of  the  IMU  does  not  yield  any  reliable  motion 
data,  sensors  fusion  at  this  point  of  time  is  obsolete.  Some  literature  research  has  been  done  to 
obtain  a  hint  as  to  how  to  fuse  the  data.  Almost  all  papers  related  to  sensor  fusion  utilize  the 
extended  Kalman  filter.  Welch  [16]  provides  a  decent  introduction  in  Kalman  filtering.  Nonetheless, 
it  is  anticipated  that  Neural  Networks  might  be  applicable  to  this  problem  as  well.  Thus,  the  aspect 
of  sensor  fusion  will  be  left  for  future  work. 
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VII.  CONCLUSIONS  AND  RECOMMENDATIONS 

FOR  FUTURE  WORK 

A.  CONCLUSIONS 

The  research  issues  addressed  by  this  thesis  were 

•  Implement  the  hardware  and  software  for  an  Inertial  Measurement  Unit 

•  Implement  the  software  for  a  shaft  encoder  system 

•  Evaluate  the  performance  for  both  sensors 

•  Sensor  Fusion 

Both  the  IMU  and  the  shaft  encoder  systems  have  been  implemented  in  software  and  hardware.  The 
sampling  frequency  for  the  A/D-Board  was  set  to  be  5  kHz.  Both  systems  have  been  tested  with 
three  different  linear  motion  profiles. 

The  work  conducted  in  addressing  the  first  of  these  topics  revealed  several  sources  of  nav¬ 
igation  inaccuracy.  The  A/D  Converter  board  currently  in  use  does  not  match  the  IMU’s  output 
range  for  accelerations  below  about  1  m/sec^.  In  addition,  due  to  the  vehicle’s  sophisticated  wheel 
suspension,  the  IMU’s  attitude  control  could  not  be  related  to  the  attitude  of  the  vehicle  and  was 
changing  with  time  as  the  vehicle  moved.  This  introduced  a  slowly  varying  and  yet  significant  error 
in  numerically  integrating  the  acceleration. 

The  second  issue  addressed  proved  to  be  less  diflBcult.  Decent  results  have  been  obtained 
for  th  elinear  motion  under  the  condition  that  no  slip  occurs  and  the  vehicle’s  position  can  be 
determined  to  within  0.5  percent  accuracy. 

The  overall  motion  control  system  seems  to  be  stable  at  all.  However,  it  has  been  observed 
that  computation  power  for  the  68040  processor  is  scarce.  This  is  mainly  to  the  fact  that  a  public 
domain  GCC  Compiler  is  in  use  for  generating  the  executable  code.  This  compiler  does  not  seem 
to  generate  optimal  executable  code.  In  addition,  the  lack  of  a  math  processor  and  math  library 
functions  required  that  semi-optimal  trigonometric  functions  be  implemented  in  the  source  code  as 
well,  introducing  further  inaccuracies. 


B.  RECOMMENDATIONS  FOR  FUTURE  WORK 

There  are  many  issues  that  were  briefly  addressed  in  this  thesis  but  could  not  be  investigated 
in  detail.  Much  work  needs  to  be  done  iii  the  following  areas. 
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1.  Determine  the  optimal  resolution  for  the  A/D-Board  based  on  the  anticipated  motion  pro¬ 
files. 

Investigate  whether  or  not  variable  gain  control  for  the  IMU  data  would  improve  the  per¬ 
formance  of  the  IMU. 

3.  Develop  a  scheme  for  attitude  control  vice  changing  the  vehicle’s  suspension. 

4.  Implement  the  filter  algorithms  as  determined  in  this  thesis.  Care  needs  to  be  taken  that 
computation  time  is  crucial  and  efficient  computation  methods  be  used. 

5.  Implement  an  Input/Output  Kernel  utilizing  the  68030  processor  for  online  debugging, 
display  of  status  information,  and  eventually  off-loading  of  some  of  the  lower  priority  task 
such  as  transferring  data  between  boards. 

6.  Investigate  how  the  system  presented  in  this  thesis  would  work  for  most  general  type  of 
motion  including  rotational  motion  and  motion  in  three  dimensions. 
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APPENDIX  A:  CONSTANTS 


_ Table  1.1:  Constants  used  throughout  the  text 

Universal  constant  of  gravitation  G=6.672  •  10"^^ 
Mass  of  Earth  M=5.98  *  10^^  kg 

mean  Earth  radius  i?e  =6.371  *  10®  m 
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APPENDIX  B:  MATLAB  M-FILES 


This  appendix  contains  essential  MATLAB  M-Files  that  are  being  referenced  in  the  text. 


1.  IMU.M 


The  MATLAAB  file  ^  imu.m'  is  used  to  analyze  the  data  recorded  from  the  IMU.  It  makes 
use  of  the  MATLAB  functions  'filterl’,  ‘eulerl.m’  and  ^ integral^  which  are  listed  following 
this  section. 


1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 

50 

51 

52 

53 


function  imu(fnajne,G,T,f ) 


jr - 

X  function  imuCfname ,G,T,f ) 

- 

7. 

7,  M-File  to  obtain  reliable  position  data.  Procedure: 

7. 

7.  1.  load  data  and  scale  data 

7.  2.  plot  data  in  frame  {B} 

7.  3.  filter  data  with  butterworth  LP  filter  in  frame  {B} 

7t  4.  determine  Euler  angles  and  transform  data  fto  frame  -{R}- 
7.  5.  integrate  data  to  obtain  velocity 

7. 

7.  Author: 

7*  Date: 

%  Compiler: 

% 

7.  Input: 

7. 

7. 

% 

7. 

7. 

7. - 


Thorsten  Leonardy 
10/23/97 
MATLAB  V4.21C 

fname  =  name  of  data  file 

G  =  gain  sequence  for  channels,  default  [1114] 
note  that  G(3)  includes  the  orientation  of  the 
IHU’s  z-axis  (>0  is  up,  <0  is  down) 

T  =  sampling  time  for  data 
f  =  switch  for  filtering  ax  data 


g=9.81; 


7»  local  gravitational  constant  [g=9. 81m/s"2] 


if  nargin<2 
G=[l  114]; 
T=0.01; 
f=0; 

end 


7t  sample  gain 

7.  samples  per  block  and  channel 
7t  do  not  filter  data 


up  =  G(3)/abs(G(3))  %  determine  if  IMU’s  z-axis  points  up 

G(3)=abs(G(3)); 


7*  load  data,  ax, ay  and  az  are  in  [m/sec"2]  or  [g]  ,  wz  is  in  [rad/sec] 

[t ,  ax ,  ay ,  az , wz] -getdat a (f name , G ,T)  ; 

disp(’»>  Plot  data  in  <B>  .  .  .  *) 

plotdata(t,ax,ay,az,wz) ;  X  plot  data 

disp(*»>  Transform  <B}  — >  iR>  ...’) 

[ax,ay,az]=fiulerl(ax,ay ,az,up)  ;  X  transform  data  to  reference  frame  {A} 

disp(*»>  Plot  data  in  <R>  ...*) 

plotdata(t,ax,ay,az,wz) ;  X  plot  data  in  {R} 


disp(*»>  Integrate  data  in  {R}  to 
[tv,vx]=integral(t ,g#ax,l) ; 
[tv,vy]=integral(t ,g*ay,l) ; 
[tv,vz]=integral(t,g*(az-up) ,1) ; 


obtain  v  . . . * ) 

X  integrate  step  by  step 
X  integrate  step  by  step 
X  integrate  step  by  step 
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56 

57 

58 

59 

60 
61 
62 

63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 

76 

77 

78 

79 

80 
81 
82 

83 

84 

85 

86 

87 

88 

89 

90 

91 

92 

93 

94 

95 

96 

97 

98 

99 
100 
101 
102 

103 

104 

105 

106 

107 

108 

109 

110 
111 
112 

113 

114 

115 

116 

117 

118 

119 

120 
121 
122 

123 

124 

125 


figure 

myplot (tv, vr, » Velocity  in  frame  [m/sec]  \  [3  1  1]) 

myplot(tv,vy,»»,»\’v_y  [m/sec]  \  [3  1  2]) 
myplot(tv,vz,»».»t  [sec]  \  »v_z  [m/sec] », [3  1  3]) 

disp(»»>  Integrate  data  in  {R}  to  obtain  position  ...0 
[tp, x] -integral (tv, vx,  1)  ;  •/,  integrate  step  by  step 

[tp,y]=integral(tv,vy,l) ;  %  integrate  step  by  step 

[tp,z]=integral(tv,vz,l)  ;  7,  integrate  step  by  step 

figure 

myplot(tp,x, ’Position  in  frame  {R>’,”,’x  [m]  ’ ,  [3  1  1]) 
myplot(tp,y,”,”,’y  [m]  ’ ,  [3  1  2]) 
myplot(tp,z,”,’t  [sec]’,»z  [m]  ’ ,  [3  1  3]) 


- - 

L  filter  the  data  for  acceleration  in  x  direction 

- - 

if  f 

mx=mean(ax) ;  %  compute  the  mean 

my=mean(ay);  %  compute  the  mean 

mz=Tnean(az) ;  %  compute  the  mean 


7t  compute  the  FFT 
[AX,f3=filterl(ax,6,t(2)“t(l)) ; 

mAX=AX(l);  \  obtain  the  mean 

AX(1)=0;  7,  suppress  dc  component 

figtire 

myplot(t,  ax.  [’Acceleration  ax  in  frame  {R},  mean  is  ’  num2str(mx)] , ’t  [sec]»,»ax  [g]  ’ , 

myplot(f  ,AX.  [’FFT  for  ax  [g]  ,  mean  is  AX(f=0)=  ’  num2str(mAX)  ’  [g]  ,  fE=5000  Hz’] 

»f  [Hz]’, ’AX  [g]’.[3  1  2]) 

7t  zoom  on  in  for  f=0..50  Hz 
ii=f ind(f <=50) ; 

myplot(f (ix) , AX(ix) , ’Blow  up  view  for  FFT  for  ax  [g] ’ , . . . 

*f  [Hz]’, ’AX  [g]’.[3  1  3]) 


- - 

7.  filter  the  data 

% - - - 

af=filterl(ax, 10, 20/2500, 50/2500, 0.1, 80);  7.  Cauer  filter 

figure 

myplot(t,af, ’Acceleration  ax  in  frame  {R}  after  elliptic  filtering’,.. 
*t  [sec]’, ’ax  [g]’,[2  1  1]) 


- - 

7.  Integrate  ax 

- - 

[t,v]=integral(t,af ,6) ; 

myplot(t,v, ’Velocity  vx  in  frame  {R}  after  elliptic  filtering’,... 

’t  [sec]’,’vx  [m/s] ’,[2  12]) 

end  7.  of  if  f 

disp(’»>  Plot  all  figures  to  disk  in  postscript  format  as  ’’fname  xxx.ps’”) 
for  i=l:gcf 
figure(i) 

eval( [’print  -dps2  ’  fname  ’_’  num2str(i)  ’.ps’]) 
end 

return 

- - 

7.  end  of  ’imu.m’ 


126  7. 


1  1]) 
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FILTERl.M 


The  file  ‘filterl’  provides  a  set  of  suitable  filter  routines  such  as  an  FFT,  Chebychev  or 
Butterworth  filter,  and  more. 

1  function  Cy,f]=filterl(x,type,a,b,c,d) 

2  - - - 

3  7,  fimction  Cy,f]=filterl(x,type,a,b,c,d) 

4  % - 

5  7,  Author:  Thorsten  Leonardy 

6  7.  Date:  10/16/97 

7  7.  Compiler:  MATLAB  V4.2cl 

8  7. 

9  7.  Input:  X  =  input  data  matrix  (M*N) 

10  7.  “type  =  utility  function  (filter)  to  apply 

11  7»  a..d  =  parameter  used  for  some  filter  types 

12  7. 

13  7i  type  2 . .  4  average  across  the  rows : 

14  7  "type  =  2  simple  mean 

15  7  ^ype  =  3  average  using  Simpson’s  3/8  rule 

16  7  type  =  4  average  using  Simpson’s  1/3  rule  on  9  samples 

17  7  “type  =  5  average  using  trapezoidal  rule 

18  7.  type  6  operate  on  each  row: 

19  7  type  =  6  obtain  Fourier  transform  (a  is  the  sample  interval  in  [sec]) 

20  7  type  7  ...  9  operate  on  first  row  only: 

21  7  "type  =  7  moving  average  FIR-Filtcr  [n  Taps] 

22  7  ^yp®  '  8  Butterworth  filter  [wp » ws , Rp ,  Rs] 

23  7  "typ®  =  9  Chebychev  Filter  [wp« ws,Rp,Rs] 

24  7  "typ®  =  10  Elliptic  (Cauer  Filter)  [wp ,  ws , Rp , Rs] 

25  7 

26  7  Output:  y  =  output  data  (M*N2/2) , 

27  7  N2  is  a  power  of  two  closest  to  and  less  or  equal  to  N 

28  7  f  =  frequency  scale  (l*N2/2)  for  y  if  type=10 

29  7  - 

30 

31  disp ([’♦*♦  Function  "filterl",  type  ’  num2str(type)  ’  »**’]) 

32 

33  if  type==0 

34  y=x; 

35  return 

36  end 

37 

38  7  compute  mean  of  the  sampled  data  from  the  channel 

39  if  type“l 

40  y=x(a,:); 

41  end 

42 

43  if  type— 2 

44  y=mean(x) ; 

45  end 

46 

47  if  type—3 

48  c=(3/8)*[l  332332331]; 

49  y=c*x/9; 

50  end 

51 

52  if  type==4 

53  c=(l/3)*[l  42424241]; 

54  y=c*x(l:9,:)/8; 

55  end 

56 

57  if  type==5 

58  c=(l/2)*Cl  222222221]; 

59  y=c*x/9; 

60  end 

61 

62  7  - r - 

63  7  Fourier  Transform  of  x 

64  7  - 

65 

66  if  type==6 

67 

68  T=a;  7  sampling  time  of  data 


61 


62 


y,  filter  only  first  row 


145 

146 

147  y,  filter  specifications  (digital  frequencies) 

148  y  e.g.  if  fs=:2000Hz  and  passband  edge  is  supposed  to  be  at  fp=500  Hz, 

149  y  pEirameter  wp  must  be  wp=fp/(f s/2)*500/(2000/2)»0. 5  !!! 

150  wp=a;  '/,  wp  is  passband  edge  [0..1]  where  1  relates  to  fp/(fs/2)  ... 

151  ws=b;  y  stopband  edge  ... 

152  Rp=c;  y  ...  and  max.  attenuation  [dB]  at  passband  edge 

153  Rs=d:  y  —  and  min.  attenuation  [dB]  at  stopband  edge 

154 

155  [N,wn]=cheb2ord(wp,ws,Rp,Rs)  ;  */,  filter  order  and  3dB  cut  off -frequency 

156  disp([’Chebychev  Type  II  filter  order  *  num2str(N)]) 

157 

158  [b,a]=cheby2(N,Rs,wn) ;  7,  compute  the  filter  coefficients 

159  y=filt€r(b,a,x) ;  7.  filter  the  data 

160 

161  end 

162 

163  y - 

164  y  Elliptic  filter  (Cauer  filter) 

165  y - 

166  if  type==10 

167 

168  x=x(l,:);  7,  filter  only  first  row 

169 

170  y  filter  specifications  (digital  frequencies) 

171  y  e.g.  if  fs=2000H2  and  passband  edge  is  supposed  to  be  at  fp=600  Hz, 

172  y  parameter  wp  must  be  wp=fp/(f 5/2) =500/ (2000/2) =0.5  !!! 

173  wp=a;  y  wp  is  passband  edge  C0..1]  where  1  relates  to  fp/(fs/2)  ... 

174  ws=b;  y  stopband  edge  ... 

175  Rp=c;  y  ...  and  max.  attenuation  [dB]  at  passband  edge 

176  Rs=d;  7,  ...  and  min.  attenuation  [dB]  at  stopband  edge 

177 

178  [N,Wn]=ellipord(wp,ws,Rp,Rs) ;  7,  filter  order  and  3dB  cutoff-frequency 

179  disp( [’Elliptic  filter  order  ’  num2str(N)]) 

180 

181  [b,a]=ellip(N,Rp,Rs,Wn) ;  7.  compute  the  filter  coefficients 

182  y=filter(b,a,x) ;  7,  filter  the  data 

183 

184  end 

185 

186 

187  return 

188 

189  y - 

190  y  end  of  ’filterl.m* 

191  y - 


3.  EULERl.M 


The  function  ‘eulerl.m’  is  used  to  convert  the  recorded  IMU  data  which  is  given  in  the 
sensor  frame  {S}  to  the  reference  frame  {R}  by  means  of  rotation  matrices. 

1  function  [euc,ay,az]=euleri(ax,ay,az,up) 

2 

3  - 

4  y  function  [ax,ay,az]=eulerl(ax,ay,az,up) 

5  - 

6  7. 

7  y  H-File  for  computing  the  Euler  angles  for  a  given  set  of  data 

8  y  measured  in  the  sensor  frame  -[S}  and  transforming  the  data  into 

9  7,  the  reference  frame  -[R}. 

10  7. 

11  y  Author:  Thorsten  Leonardy 

12  y  Date:  10/16/97 

13  y  Compiler:  MATLAB  V4.21c 

14  y 

15  y  Input:  ax(l,N)  =  acceleration  [g]  in  <S}  ax-direction 

16  y  ay(l,N)  =  acceleration  [g]  in  -CS}  ay-direction 
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az(l,N)  =  acceleration  [g]  in  {S>  az-direction 
/•  =  orientation  of  sensors  z-axis  (+l=up,“l*down) 

19  / 

20  7,  Return:  acceleration  relative  to  frame  {R} 

21  % - 

22 

23  7,  put  data  into  one  measurement  matrix  aS(3,N)  relative  to  Frame  {S> 

24  aS=Eai;ay;az] ; 

25 

26  % - 

27  7t  determine  the  Euler  angles  based  on  the  average 

28  7i  acceleration  during  2nd  second 

29  - - 

30  ii=101:200;  %  may  change  this 

31  m=mean(aS(:,ix)0;  7.  take  the  mean  of  first  ix  values 

32  g=sqrt  (m*m*)  ;  7#  the  gravity  based  on  the  mean 

33  dispCC’ — >  mean  of  g  in  frame  {S}  is  ’  num2str(g,6)  '  g*]) 

35  psi=0.0;  •/,  arbitrary  value 

36  phi=-asin(m(l));  %  phi 

37  theta=asin(m(2)/cos(phi))  ;  7,  theta 

38 

39  phi=up*phi ; 

40  theta*up*theta ; 

41 

42  disp([»— >  Theta  (roll)  is  »  num2str (theta* 180/pi, 7)  »  degrees*]) 

43  disp([*— >  Phi  (pitch)  is  *  num2str (phi* 180/pi, 7)  »  degrees*]) 

44  disp(C*— >  Psi  (yaw)  is  »  num2str(psi*180/pi,7)  *  degrees*]) 

46  % - 


47  ft  compute  elements  of  the  rotation  matrix 

48  7.  complete  rotation  matrix  would  be  R=RZ*Ry*RX 


49 

7. - 

50 

51 

RX=C  1 

0 

0  ; 

Z  rotation  matrix  about  X  A 

52 

0 

cos (theta) 

-sin(theta)  ; 

53 

0 

sln(theta) 

cos (theta)  ]; 

54 

55 

RY=[  cos (phi) 

0 

sin (phi)  ; 

Z  rotation  matrix  about  Y  A 

56 

0 

1 

0  ; 

57 

-sin(phi) 

0 

cos (phi)  ]; 

58 

59 

RZ=[  cos (psi) 

-sin(psi) 

0  ; 

Z  rotation  matrix  about  Z_A 

60 

sin(psi) 

cos (psi) 

0  ; 

61 

0 

0 

1  ]; 

62 

63 

Z - 

64  Z  rotate  the  data  successively  to  frame  lA} 

65  - - 

66  aR=RX*aS;  7,  rotate  {B}  about  {R}  x-axis 

67  aR=RY*aR;  5i  rotate  new  {B}  about  <R}  y-axis 

68  aR=RZ*aR;  %  rotate  new  iBl  about  -{Rl  z-axis 

69 

70  m=mean(aR(: ,ix) *) ;  %  take  the  mean  of  first  ix  values 

71  g=sqrt(m*m*) ;  7  the  gravity  based  on  the  mean 

72  disp(C*“>  mean  of  g  in  frame  {A}  is  *  num2str(g,6)  *  g*]) 

74  ax=aR(l,:); 

75  ay=aR(2,:); 

76  az=aR(3,:); 

77 

78  return 

79  )r - 

80  Z  end  of  *eulerl.m* 

81  - - 
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4, 


INTEGRAL.M 


This  function  implements  the  Newton-Cotes  integration  formulas  as  described  in  the  text. 


This  provides  an  easy  means  to  compare  the  results  for  different  integration  schemes. 

1  ftinction  [t ,y]=integral(t,x,n) 


- 

y,  fimction  [t ,y]=integral(t ,i,n) 

y. 

%  Integrates  the  input  x  based  on  the  Newton-Cotes  algorithm, 
y.  The  integral  is  computed  on  each  column. 

y. 

y  n  =  the  number  of  panels  (n  panels  require  n+1  data  points) 
y  t  is  the  time  base  corresponding  to  the  data. 
y^ - - 


13  [N,c]=size(t) 

14 

15  if  (ON) 

16  t=t’;  N=c;  y  need  data  as  a  vector,  N=length  of  data 

17  end 

18 

19  y  prepare  the  coefficients  in  the  sum  formula 

20  if  (n==l),c=[l  l]/2;  end 

21  if  (n“2),c=Cl  2  l]/6;  end 

22  if  (n=>3).c=*[l  3  3  l]/8;  end 

23  if  (n==4),c=[7  32  12  32  7]/90;  end 

24  if  (n==5),c=[19  75  50  50  75  19]/288;  end 

25  if  (n==6),c=[41  216  27  272  27  216  41]/840;  end 

26  c=n*(t(2)-t (l))*c; 

27 

28  for  i=l:n:N-n 

29  x(i,  :)*c#x(i:i+n, :)  ;  */,  store  result  in  place 

30  end 

31 

32  y=cumsum(x(l:n:N-n, : )) ; 

33  t«t  (n+1  :n:N)  ;  7,  return  the  time  scale 

34 

35  return 

36  y - 

37  y  End  of  *integral.m* 

38  7. - 


5. 


SHAFT.M 


In  order  to  analyze  the  shaft  encoder  data  that  was  recorded  during  the  different  motion 


programs. 

1  function  shaft  (f name) 

2 


3  y 


function  shaft (f name) 


M-File  to  analyze  the  shaift  encoder  readings  recorded  for  SHEPHERD’S 
motion  according  to  the  different  motion  profiles. 


Author:  Thorsten  Leonardy 

Date;  11/11/97 

Compiler:  MATLAB  V4.21c 


8  y 

9  y 

10  y 

11  y 

12  y 

13  y 

14  y 

15  y 

16  y - 

17 

18  y  load  data 


Input ; 


fname  =  naune  of  data  file  (no  extension  ’  +  .dat’) 
e.g.  at  the  prompt  »shaft(*linear4*) 
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19 

20 
21 
22 

23 

24 

25 

26 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 

39 

40 

41 

42 

43 

44 

45 

46 

47 

48 

49 

50 

51 

52 

53 

54 

55 

56 

57 

58 

59 

60 
61 
62 

63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

75 


eval([>load  -ascii  ’  fname  '.dat,  data=*  fname  fname  ’=□;*]) 


X  reshape  the  data 
Nslength (data) /8 
data=reshape(data,8,N) ; 
t=0.01*(l:N); 

driveDelta=data(l:2:8, : ) ’ 
steerDelta=data(2:2:8, : ) ’ 


7.  number  of  10ms  intervals  contained  in  data 

%  the  time  base 

7.  driving  data  [counts/lOms) 

7.  steering  data  [count s/lOmsj 

7.  account  for  the  fact  that  drive  encoders  for  wheels  2  and  4  read  negative 
X  differences  if  wheels  are  driving  forward 

driveDeltaC: ,2:2: 4)=-driveDelta(: ,2:2:4) ; 

/.  accumulate  the  data  to  obtain  true  rotation  of  motors 
drive=cumsum(driveDelta);  7.  the  distance  travelled 

steer=cumsum(steerDelta);  %  the  angle  steered 

X  scale  to  SI  units 
drive=drive/87914 ; 
steer*steer/256; 


X  drive  distance  in  [m] 

X  angle  steered  in  degrees 


,i));  axis (a) 


X  plot  data 
figure 
for  i=l:4 

if  (mod(i,2)) 

subplot (2, 2, i+1) 
else 

subplot (2, 2, i“l) 

end 

plot (t,drive( : ,i))  , grid 
title ([’Wheel  *  num2str(i)3 , ’FontSize’ ,8) 
xlabeK’Time  [sec]  * ,  ’FontSize  ’  ,8) 
ylabeK ’Drive  distance  [m] ’ , ’FontSize’ ,8) 
set(gca, ’FontSize* ,6, ’Box’, ’off ’) 
a=axis;  a(3)=Tnin(drive(  :  ,i))  ;  a(4)=max(drive(  : 

end 

eval([ ’print  “dps2  shaft’  num2str(gcf)  ’.ps’]) 
figure 

plot (t, steer) ,grid 

title(’Steer  values  for  Wheels  1..4  with  steer  value  set  to  zero* , ’FontSize 

xlabeK’Time  [sec]  ’ ,  ’FontSize’  ,8) 

ylabeK ’Steer  angle  [degrees] ’, ’FontSize’ ,8) 

set (gca , ’FontSize ’ , 6 , ’Box ’ , ’ off ’ ) 

ix=min(f  ind(t>s=65) ) ; 

for  i=l:4 

text (t(ix) , steer (ix,i) , [’Wheel’  num2str(i)] , . . , 

’HorizontalAlign’ ,  ’left’ ,  ’VerticalAlign’ ,  ’top’ ,  ’FontSize’  .6) 

end 

eval([ ’print  -dps2  shaft 


return 
X - 


num2str(gcf)  ’.ps’]) 


X  end  of 
X - 


'shaft .m’ 


.8) 
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APPENDIX  C:  GCC  COMPILER  SOURCE-FILES 

This  appendix  lists  the  C-source  code  that  is  being  referred  to  throughout  the  text.  Each 
individual  source  file  was  written  in  C  and  crosscompiled  using  the  GCC  Compiler  Version  2.72  with 
the  following  command  line: 


gcc  -c  -m68040  -o  filename.o  filename.c 


1.  IMU.C 

The  file  ‘imu.c’  provides  all  the  routines  required  to  implement  the  inertial  measurement 
sensor  as  outlined  in  Chapter  V.  Moreover,  they  provide  the  interface  for  further  development  of 
the  system. 


Environment:  GCC  Compiler  v2.7.2 
Last  update:  10  September  1997 
Name :  Thorsten  Leonardy 

Purpose:  Provides  routines  required  for  controlling  the  inertial 

measurement  sensor. 

Compiled:  >gcc  ~c  -m68040  -o  navigat.o  navigat.c 


Here  is  hov  the  routines  work: 


README 


1 .  Make  sure  that  initVH£9325  is  called  inside  mainO 

this  will  setup  the  proper  interrupt  handling  for  reading  data 
from  the  accelerometer. 

2.  A/D-Block  conversions  as  specified  in  initVHE9325  will  be  initiated  with  every 
10ms  timer  interrupt .  However »  to  make  the  data  available ,  make  sure  that 
interrupt  for  conversion  complete  are  being  issued: 

3.  Call  start VME9325  to  enable  block  conversion  complete  interrupts 

on  IRQ-5  to  68040  processor  and  therefore  copy  data  into  main  memory 

4.  To  seize  copying  data  into  main  memory,  call  stopVME9325 

5.  The  A/D  converter  is  setup  such  that  ztfter  every  10ms  timer  interrupt 
a  block  conversion  will  be  initiated.  A  total  of  AD.NUH.CONVERSIONS 
conversions  will  be  performed  on  the  four  channels  on  the  IHU 

in  the  sequence  CHO,  CHI,  CB2,  CH3,  CHO,  ... 

The  sample  time  is  set  to  be  25us  (hence,  one  specific  channel  will 
be  sampled  every  lOOus) 

6.  If  interrupts  are  enabled,  the  most  recent  data  obtained  with  every 
lOms  timer  interrupt  will  be  stored  in  the  structure  imu  as  defined 
in  SHEPHERD. H 
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66 

67 
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71 

72 
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74 

75 

76 

77 

78 

79 
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93 
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109 
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7.  The  boards  status  can  be  observed  at  the  front  peuiel: 

(a)  green  LED  is  on  ->  board  performs  A/D-Conversions ,  interrupts  enabled 

(b)  green  LED  is  off  ->  board  performs  A/D-Conversions ,  interrupts  disabled 

(c)  red  LED  is  toggling  ->  Interrupts  are  being  handled  by  the  handler, 

^  ^  .  data  is  read  from  board  into  SHEPHERD  main  memory 

(d)  red  LED  is  on/off  ->  interrupt  handler  is  not  being  called 


#include  " shepherd. h" 
#include  "imu.h" 


int  adCounter;  /*  counter  for  debugging  purposes  ♦/ 

int  mainMemCounter;  /*  to  count  the  data  stored  in  main  memory  */ 


/*  the  next  is  used  as  temporary  storage  for  analyzing  acceleration  DATA  ♦/ 
unsigned  short  *mainMemData; 


/♦ 

♦ 

* 

* 

* 

♦ 


initVME9325(void) 

Environment:  GCC  Compiler  v2.7.2 
Last  update:  24  July  1997 
Name :  Thorsten  Leonardy 


*  Purpose:  Initializes  AD-Board  VME9325.  Board  will  convert 

*  analog  data  from  channels  specified  and  store  the  respec- 

*  tive  digital  data  (2  Bytes  per  channel,  12  bit  data,  lowest 

*  nibble  is  zero)  sequentially  in  dual  port  ram. 

*  Board  will  operate  in  Block  mode  with  interrupts  and  timed 

*  periodic  triggering  (10ms  cycle).  E.g.  perform  10  conver- 

*  sions  on  each  of  the  four  channels .  Once  40  conversions  are 

*  made,  initiate  interrupt  to  read  data  into  main  memory  and 

*  eventually  smooth/filter  data. 


void  initVME9325(void) 

< 


unsigned  char  ♦ad  =  (unsigned  char*)  VME9325_BASE ;  /♦  base  address  ♦/ 

unsigned  char  ♦vmeICR4  =  (unsigned  char*)VIC_IRQ4;  /*  VHE  ICR  IRQ-4*/ 

long  *vadr,  x-t  Vector  base  address  */ 

*(ad+0x81)=0xl0;  /*  software  reset  */ 

*(ad+0x81)-0x02;  /*  turn  both  LEDs  on  to  indicate  board  undergoes  */ 

/*  initialization  */ 


*  Interrupt  settings  for  VIC 


vadr=(loug*)0xffe40158;  /*  VBA  address  for  interrupt  handler  (4  ♦  0x56  =  0x158) 
*vadr=(long)handlerVME9325;  /*  write  address  of  handler  into  Vector  Table  */ 

/*  set  up  VIC  interface  for  VME-Bus  interrupts  to  TUARUS.  AD-Board  asserts  */ 
/*  IRQ-4  upon  interrupt  to  VME-Bus.  Route  as  IRQ-2  to  HC68040.  CAUTION  !!!  */ 
/*  make  sure  jumper  J7  on  AD-Board  is  set  correctly  !!!  ♦/ 

*vmeICR4=0x82;  /*  disable  VME-Bus  IRQ4  input,  route  as  IRQ-2  to  Processor  ♦/ 

♦ (ad+0i83) =0x56 ;  /*  interrupt  vector  number  provided  by  board  to  VIC  */ 


/*  program  scan 
/*  channels  are 
/*  *(ad+0x87) 
/*  *(ad+0x87) 
*(ad+0x87)=0x60 
*(ad+0x87)=0x61 
*(ad+0x87)*0x02 
♦(ad+0x87)=0xc3 


*/ 

*/ 


sequence  (may  wish  to  arrange  channels  to  be  scanned  different 1 
scanned,  converted  and  stored  in  memory  in  this  order 
-0x00;  /*  channel  0  (ax,  +-7.5V  input  range,  gain  xl) 

=0x01;  /*  channel  1  (ay,  +-7.5V  input  range,  gain  xl) 

/*  channel  0  (ax,  +-7.6V  input  range,  gain  x8)  */ 

/*  channel  1  (ay,  +-7.6V  input  range,  gain  x8)  */ 

/*  channel  2  (az,  +-7.5V  input  range,  gain  xl)  */ 

/*  channel  3  (wy,  +-2.6V  input  range,  gain  x4)  */ 

/*  gain  x4  to  cover  max.  input  range  +-10V,  */ 

/*  set  EOS  bit  to  indicate  end  of  scan  sequence*/ 


*/ 


*/ 

*/ 
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/♦  setup  Board  Control  Register  */ 

*(ad+0x85)=0x08;  /*  enable  timer  circuit,  enable  interrupts  */ 

/♦  block  mode,  software  initiates  very  first  trigger  */ 

/*  setup  timed  periodic  triggering  circuit  for  SOusec  (  T  =  10  ♦  10  /  2  MHz  )♦/ 
♦(ad+0x8f )=0x54;  /*  setup  counter  to  receive  single  byte  prescaler  count  */ 

♦(ad+0i8b)=0x0A;  /*  load  prescaler  value  into  Timer  Prescaler  Register  ♦/ 

♦(ad+0x8f)=0x94;  /*  setup  counter  to  receive  single  byte  timer  count  ♦/ 

♦(ad+0x8d)=0x0A;  /♦  load  Conversion  Timer  Register  ♦/ 


/*  load  conversion  count  register  ♦/ 

*( (unsigned  short  ★) (ad+0x90) )=200; 

/*  initialization  is  complete  */ 

*(ad+0x81)=0x01;  /♦  turn  off  both  LED,  disable  interrupts  */ 

sioOut  (0, ’‘A/D-Board  initialized\n\r*') ; 
return ; 

}  /♦  end  of  AD_Init  */ 


/* - * 

*  analyzeVME9325  * 

♦  ♦ 

♦  Environment:  GCC  Compiler  v2.7.2  * 

♦  Last  update:  24  July  1997  ♦ 

*  Name:  Thorsten  Leonardy  * 

*  ♦ 

*  Purpose:  Saves  the  data  for  one  complete  block  conversion  cycle  from  ♦ 

*  dual-port  RAM  of  A/D-Board  to  Shepherd’s  main  memory.  ♦ 

*  In  the  future,  this  routine  shall  be  utilized  to  analyze  * 

*  and  filter  the  data  and  save  only  the  filtered  data.  ♦ 

*  This  is  called  from  the  interrupt  handler  routine  ♦ 

*  AD.Handler .  ♦ 

*  * 

» - ♦/ 


void  analyzeVME9325(void) 

unsigned  short  ♦ad;  /♦  base  adless  for  data  ♦/ 
int  i ; 

unsigned  short  adDataCAD_NUM_C0NVERSI0NS]  ; 


ad=  (unsigned  shorts) VME9325.DATA;  /♦  load  base  address  for  dual  port  RAM  ♦/ 


/♦ - - * 

♦  here  goes  the  filtering  ...  ♦ 

♦  - ♦/ 

if  ((adCounter%5)*=0) 


toggleVHE( (unsigned  char  ♦)0xfd800000,0x01) ;  /♦  toggle  red  LED  every  50  msec*/ 
adCounter++; 

/* - ♦ 

♦  This  is  temporary  backup  * 

♦  - »/ 

for  (i=0;  i<AD.NUM_COHVERSIONS ;  i++)  { 

adData[i]=*ad++;  /♦  neglect  lower  nibble  ♦/ 

♦mainMemData++=adData[i]  ;  /*  save  data  in  main  memory  ♦/ 

> 

#ifdef  0 

/♦  once  data  is  filtered,  store  obtained  values  in  imu  ♦/ 

imu . ax^adData [0] ; 

imu. ay=adData[l] ; 

imu . az^adOat  a [2] ; 

imu . omega^z-adData [3] ; 

«endif 

/♦  reload  start  conversion  register  for  next  block  conversion  ♦/ 
ad~ (unsigned  shorts) 0xfd800090;  /♦  address  for  SCR  ♦/ 

♦ad-AD_NUM_CONVERSIONS;  /♦  reload  register  ♦/ 
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return; 

}  /*  end  of  analy2eVME9325  ♦/ 


*  start VHE9325 (void) 


*  Environment:  GCC  Compiler  v2.7.2 

*  Last  update:  10  September  1997 

*  Name:  Thorsten  Leonardy 


*  Purpose: 


enables  interrupts  issued  by  the  VHE9325  board. 


♦  Called  from:  whatever  function. 


void  startVME9325(void) 


unsigned  char  *statusRegister=  (unsigned  char  ♦)VHE9325_BASE+0x0081; 
unsigned  char  ♦vmeICR4  =  (unsigned  char*)VIC_IRQ4 ;  /♦  VME  ICR  IRQ-4*/ 


/*  initialize  global  variables  ...  */ 

mainHemData=(unsigned  short  ♦)IMU_DATA_ADR;  /*  start  address  for  data  storage  */ 
a  ounter-0,  counter  for  debugging  purposes  ♦/ 

♦vmeICR4=0x02;  /*  enable  VME-Bus  IRQ4  input,  route  as  IRQ-2  to  Processor  */ 

/*  write  status  register  to  enable  interrupt  and  turn  off  red  LED  */ 

*statusRegister*0x09;  /♦  turn  off  both  LEDs,  enable  interrupts  */ 


return; 

>  /♦  end  of  startVME9325  */ 


*  stopVME9325(void) 


*  Environment: 

*  Last  update: 

*  Name: 

* 

*  Purpose: 


♦  Called  from: 


GCC  Compiler  v2.7.2 
10  September  1997 
Thorsten  Leonardy 

disables  interrupts  off  the  VHE9325  AD-Board.  Yet,  board 
will  still  perform  A/D-Conversions  but  data  will  not  be 
made  available  to  the  operating  system. 


void  stopVME9325(void) 

< 

unsigned  char  ♦statusRegister=  (unsigned  char  *)VME9325^BASE+0x0081; 
unsigned  char  *vmeICR4  =  (unsigned  char*)VIC_IRQ4;  /♦  VME  ICR  IRQ-4*/ 


#ifdef  0 

/♦  initialize  global  variables  . . .  ♦/ 
mainNemData=  (unsigned  short  *)IHU_DATA_ADR; 
adCounters=0; 

#endif 


/*  start  address  for  data  storage  */ 
/*  counter  for  debugging  purposes  */ 


*vmeICR4-0x82;  /*  disable  VME-Bus  IRQ4  input,  route  as  IRQ-2  to  Processor  */ 

/*  write  status  register  to  disable  interrupt  and  turn  off  red  LED  ♦/ 
*statusRegister=0x01;  /♦  turn  off  both  LEDs,  disable  interrupts  */ 

return; 

}  /*  end  of  stopVHE9325  */ 


Assembler  routines 


70 


270 

271 

272 

*  handlerVME9325 

273 

* 

274 

♦  Environment : 

GCC  Compiler  v2. 

7.2 

275 

*  Last  update: 

10  September  1997 

276 

*  Nzune: 

Thorsten  Leonardy 

277 

* 

278 

*  Purpose: 

Handles  the  VME- 

■Bus 

279 

* 

280 

t 

281 

282 

283 

asm(‘' 

284 

.even 

285 

.text 

286 

.globl  _ 

handlerVME9325 

287 

288 

_handlerVHE9325 

289 

290 

291 

link 

a6,#-184 

/* 

292 

fsave 

a6fi(-184) 

293 

#ifdef  0 

294 

fmovemx 

fp0-fp7,sp®“ 

/* 

295 

fmovel 

f per , spfi- 

/* 

296 

fmovel 

f psr , spC- 

/* 

297 

fmovel 

fpiar,spfi- 

/* 

298 

tendif 

299 

moveml 

d0’'d7 /a0-a5 ,  spO- 

/* 

300 

301 

addq . 1 

#l,.adCounter 

/* 

302 

303 

move .  1 

#0xfd800081,a0 

/* 

304 

and.b 

#0xfd, (aO) 

/* 

305 

306 

move . 1 

#0xfd800090,a0 

/* 

307 

move . w 

#200, (aO) 

308 

309 

310 

#ifdef  1 

311 

312 

move . 1 

#0xfd820000,a0 

/* 

313 

lea 

.mainMemData , al 

314 

move . 1 

(al),a2 

315 

316 

clr.l 

dO 

/* 

317 

318 

_loop : 

319 

cmp.l 

#199, dO 

320 

ble.b 

.proceed 

321 

nop 

322 

bra.b 

.done 

323 

nop 

324 

325 

.proceed: 

326 

327 

move . w 

(aO) ,dl 

/* 

328 

nop 

/* 

329 

move . w 

dl,(a2) 

330 

nop 

331 

addq.l 

#2.a0 

/* 

332 

addq.l 

#2,a2 

/* 

333 

addq . 1 

#l,d0 

/* 

334 

bra.b 

.loop 

335 

336 

.done : 

337 

338 

move . 1 

a2,(al) 

/*  t 

339 

340 

341 

/*  jsr 

_analyzeVME9325 

y 

342 

/♦ 

343 

#endif 

344 

345 

/*  allocate  184  Bytes  on  stack  to  save  registers  */ 


♦/ 

*/ 

♦/ 


/*  reload  start  conversion  register  ♦/ 


/♦  load  address  for  dual  port  RAM  */ 


/♦  loop  counter  */ 


/♦  write  back  the  next  main  memory  location  */ 
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346 

347 

348 

349 

350 

351 

352 

353 

354 

355 

356 

357 

358 

359 

360 

361 

362 

363 

364 

365 


moveml  spfi+ , dO-d7/aO-a5 
#ifdef  0 

fmovel  spfi+,fpiar 
fmovel  sp<D+,fpsr 
fmovel  sp0+,fpcr 
fmovemx  sp0+,fpO-fp7 

#endif 

frestore  a6®{-184) 
unlk  a6 

rte 


End  of  imu.c 


2.  MOTOR.C 


The  file  ‘motor.c’  provides  the  routines  required  to  control  the  servo  motors.  Although  the 

listing  was  already  given  by  Mays/Reid  [1],  some  changes  had  beend  done  to  improve  the  overall 
execution  time. 

2  //  Edward  Hays 

3  //  Shepherd  project 

4  //  20  February  1997 

5  //  update ;  27  October  1997  Thorsten  Leouardy 

6  //  ->  provide  code  to  detect  slip, 

^  ^ ^  eliminate  calls  to  readDriveEncoders,  readSteerEncoders 

®  including  code  in  readEncoders  (improves  execution  speed) 

®  compute  speed  and  angular  velocity  immediately  inside 

10  //  readEncoders . 

11  //  MotionControl 

12  //  ======-s==®«s;s==»ss==*s;5:a:--ssss--=ass--=:ss== - -=: _ =-== _ -= _ _  ~  */ 

^3  —  -*  —  / 

14 

15  #include  "shepherd. h” 

16  #include  "motor.h" 

17  #include  "movement. h" 

18  #include  "math.h" 

19 

20  double  theta,  omega,  speed; 

21  double  a,  /#  acceleration  in  cm/sec*2  */ 

22  ddC4];  /♦  driveDelta  required  for  velocity  to  steer  ♦/ 

24  'I*  storage  for  time  it  took  to  rotate  360  degrees  [10ms]  */ 

24  short  testSpeed=0x0b00;  /*  temp  variable  for  changing  speed  ♦/ 

25  double  radPerDigit  [ARRAY.SIZE]  ; 

26  int  ddc=10000,tc=2000;  /*  desired  vale  for  driveDelta  ♦/ 


int  ♦leoData=(int  *) 0x00100000;  /*  start  data  storage  ♦/ 


void  readEncoders ()  { 

readDriveEncoders (driveReadings) ; 
readSteerEncoders (steerReadings) ; 


void  readDriveEncoders  (unsigned  long  int  array  []) 

unsigned  char  ♦p=  (unsigned  char*)VMECTRl,  cl,  c2,  c3; 
int  ix; 

long  int  temp; 
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42 

43  for  (ix=0;  ix<4;  ix++)  {  /♦  read  all  four  motors  subsequent! ally  ♦/ 

44 

45  *(p+3)=0x03;  /♦  load  output  latch  from  counter  */ 

46  ♦(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  ♦/ 

47 

48  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  ♦/ 

49  /*  first  access  to  Output  Latch  Register  reads  least  significant  */ 

50  /*  byte  first  ♦/ 

51 

52  cl  =  *(p+l)  t  OxOOff ; 

53  c2  =  *(p+l)  &  OxOOff; 

54  c3  =  *(p+l)  k  OxOOff; 

55  arrayCix]  =  ((unsigned  int)ci)|  ((unsigned  int)c2  «  8)  I 

56  ((unsigned  int)c3  «  16); 

57 

58  p=p+4;  /*  increment  pointer  for  next  counter  */ 

59 

60 

61  > 

62  return; 

63  >  /♦  end  of  re adDrive Encoders  */ 

64 

65 

66  void  readSteerEncoders (unsigned  long  int  array []) 

67  < 

68  unsigned  char  ♦p=(unsigned  char*) (VMECTRl  +  0x0100),  cl,  c2,  c3; 

69  int  ix; 

70 

71 

72  for  (ix=0;  ix<4;  ix++)  {  /*  read  all  four  motors  subsequentially  */ 

73 

74  *(p+3)=0x03;  /*  load  output  latch  from  counter  */ 

75  *(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  */ 

76 

77 

78  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

79  /*  first  access  to  Output  Latch  Register  reads  least  significant  byte  first  */ 

80 

81  cl  =  *(p+l)  k  OxOOff; 

82  c2  =  ♦(p+1)  k  OxOOff; 

83  c3  =  ♦(p+1)  k  OxOOff; 

84  arrayCix]  =  ((unsigned  int)cl)|  ((unsigned  int)c2  «  8)  I 

85  ((unsigned  int)c3  «  16); 

86 

87 

88  p=p+4;  /*  increment  pointer  for  next  counter  */ 

89 

90  } 

91  return; 

92  }  /*  end  of  readSteerEncoders  */ 

93 

94 

95 

96  void  computeActualRatesO 

97  < 

98 

99  int  i; 

100  double  count, speed; 

101 

102  for(i*0;  i<==3;  i++) 

103  < 

104  if (PreviousCountSpeed Ci]  ==  99999999)  /*  for  derivative  for  speed  */ 

105  actualSpeeds Ci]B  0.0; 

106  else 

107  ac tualSpe  e  ds [i ]  = 

108  (convertDifference((driveReading5[i]  -  PreviousComtSpeedCi])) 

109  ♦DigitTo(M)r  ive  [i]  )  /DELTA_T ; 

110  PreviousCountSpeedCi]  =  driveRcadings  [i]  ; 

111 

112  if (PreviousCount Steer [i]  “  99999999)  /*  for  derivative  for  steering  */ 

113  actualAngleRatesCi]-  0.0; 

114  else 

115  actualAngleRates [i] = 

116  (convertDifference((steerReadings[i3  -  PreviousCountSteerCi] )) 

117  ♦digitToRadSteer) /DELTA.!; 
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118 

119 

120 
121 
122 

123 

124 

125 

126 

127 

128 

129 

130 

131 

132 

133 

134 

135 

136 

137 

138 

139 

140 

141 

142 

143 

144 

145 

146 

147 

148 

149 
160 

151 

152 

153 

154 

155 

156 

157 

158 

159 

160 
161 
162 

163 

164 

165 

166 

167 

168 

169 

170 

171 

172 

173 

174 

175 

176 

177 

178 

179 

180 
181 
182 

183 

184 

185 

186 

187 

188 

189 

190 

191 

192 

193 


PreviousCountSteer [i3  =  steerReadings [i] ; 

> 


void  accumulateDriveSpeedO 
int  i; 

for(i=0;i<=3; i++){ 

Display^Speeds [i]  +=  actualSpeeds [i] ; 
retiirn; 

} 

void  accumulateDriveSteerO 
int  i; 

f or (i=0 ; i<=3 ; i++) { 

Display.Steers [i]  +=  10*actualAngleRates[i] ; 

^  actualAngles [i]  +=  actualAngleRates[i3*DELTA_T; 

return ; 

> 


Function  convertDifferenceO  returns  the  difference  between  the  new  shaft 
encoder  position  and  the  old  shaft  encoder  position.  The  shaft  encoder  values 
contain  only  24  bits  (OxOOOOOO-Oxffffff ) .  The  routine  adjusts  for  the  trans¬ 
ition  from  Oxffffff  to  0x000000  and  vice  versa. 


int  convertDifference(int  value) 

if (value  <  -0x800000) 
value  ft=  OxOOffffff; 
else  if (value  >-  0x800000) 
value  1=  Oxff 000000; 

return  value; 

> 


/♦ - 

*  readNewEncoder () 


*  Environment;  GCC  Compiler  v2.7.2 

*  Name:  Thorsten  Leonardy 

*  Last  update:  10/27/97 

*  Purpose:  This  function  reads  the  counter  status  for  drive  and  steer 

*  motors  every  10ms  and  stores  the  current  values  in  the 

*  variables  ’driveReadings*  and  ’steerReadings*.  In  addition, 

*  the  incremental  change  to  the  last  update  is  stored  in  the 

*  v^iables  ’driveDelta*  and  ’steerDelta’  to  allow  for  compu- 

*  ting  the  most  current  speeds  and  angular  velocities. 

* 

♦  Called  from:  driver ()  in  movement. c 


void  readNewEncoder () 


unsigned  char  ♦pj+d; 
int  ix; 

p= (unsigned  char* ) VMECTRl ;  /*  access  steering  counter  registers  ♦/ 

for  (ix=0;  ix<4;  ix++)  {  /♦  read  all  four  driving  motors  sequentially  */ 

driveCountPreviousCix3=driveCount[ii] ;  /♦  save  previous  value 
steerCountPrevious[ix3=:steerCountCix] ;  /*  save  previous  value 

/♦ - 
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194  /♦  read  drive  encoders  for  wheel  ix  */ 

195  /♦ - ♦/ 

196  ♦(p+3)=0x03;  /★  load  output  latch  from  counter  */ 

197  *(p+3)=0x01;  /*  initialize  two-bit  output  latch  ♦/ 


198 

199  d=( (unsigned  char*)&driveCount [ix]  )+2;  /*  start  with  LSB,  need  offset  ♦/ 

200  ♦d —  =  *(p+l)  &  OxOOff;  /*  read  LSB  first  ♦/ 

201  *d —  «  ♦(p+1)  ft  OxOOff;  /♦  read  next  byte  ♦/ 

202  *d  =  ♦(p+l)  ft  OxOOff;  /*  read  most  significant  byte  ♦/ 

203 

204  /♦ - */ 

205  /*  read  steer  encoders  for  wheel  ix  */ 

206  /♦ - */ 


207  * (p+0xl03)=0x03;  /♦  load  output  latch  from  counter  ♦/ 

208  *(p+0xl03)=0x01;  /*  initialize  two-bit  output  latch  */ 

209 

210  d=:(  (unsigned  char*)ftsteerCount  [ix]  )+2;  /♦  load  LSB  first  */ 

211  *d—  =  *(p+0xl01)  ft  OxOOff;  /♦  read  LSB  first  ♦/ 

212  ♦d—  =  ♦(p+OxlOl)  ft  OxOOff;  /*  read  next  byte  ♦/ 

213  *d  =  ♦(p+OxlOl)  ft  OxOOff;  /*  read  most  significant  byte  */ 

214 

215  p=p+4;  /♦  increment  pointer  for  next  motor*/ 

216 

217  /*  determine  difference  between  previous  and  current  encoder  reading  */ 

218  steerDelt  a  [ix]  *=  (st  eerCount  [ix]  -steerCountPr  evious  [ix]  )  /256 ; 

219  driveDeltaCii]  =  (driveCount  [ix]  -drive  Count  Previous  [ix]  )/256 ; 

220 

221  /*  consider  the  fact  that  a  positive  driveDelta  for  wheels  2  and  4  ♦/ 

222  /*  indicate  that  wheel  is  driving  backwards  !!!  Thgus,  change  sign  ♦/ 

223  dr  iveDelt  a  [ix]  =  (driveCount  [ix]  -dr  iveCount  Previous  [ix] )  /256 ; 

224 

225  /*  the  following  is  just  for  testing  purposes  [leo,  11/17/97]  */ 

226  *  enc  ode  rData++=dr  iveDelt  a  [i  x]  ;  /*  store  in  main  memory  */ 

227  *€ncoderData++=steerDelta[ix] ;  /*  store  in  main  memory  */ 

228 

229  }  /*  end  of  for  ♦/ 

230 

231  /*  account  for  the  fact  that  a  positive  driveDelta  for  wheels  2  and  4  */ 

232  /*  indicate  that  wheel  is  driving  backwards  !!!  Thus,  change  sign  to  */ 

233  /*  obtain  a  positive  driveDelta  for  wheel  driving  forward  ! ! !  */ 

234  driveDelta [l]=-dr iveDelt a [1] ; 

235  dr iveDelt a [3] --dr iveDelt a [3 ] ; 

236 

237  return; 

238 

239  >  /*  end  of  readNewEncoder  */ 

240 

241 

242 


243  /* - ♦ 

244  *  readNewEncoder ()  m 

245  *  # 

246  »  Environment;  GCC  Compiler  v2.7.2  ♦ 

247  *  Name:  Thorsten  Leonardy  * 

248  *  Last  update:  10/27/97  * 


249  *  Purpose:  This  function  reads  the  counter  status  for  drive  and  steer  * 

250  •  motors  every  10ms  and  stores  the  current  values  in  the  * 

251  *  variables  ’driveReadings *  and  ’steerReadings’ .  In  addition,  * 

252  *  the  incremental  change  to  the  last  update  is  stored  in  the  * 

253  *  variables  ’driveDelta*  and  ’steerDelta*  to  allow  for  compu-  * 

254  *  ting  the  most  current  speeds  and  angular  velocities.  * 


255  *  ♦ 

256  *  Called  from:  driver ()  in  movement. c  ♦ 

257  - - #/ 


258  void  readEncoderO 

259  { 

260 

261  unsigned  char  *p,*d; 

262  int  ix; 

263 


264  p= (unsigned  char*)VMECrrRl;  /*  access  steering  counter  registers  */ 

265 

266  for  (ix=0;  ix<4;  ix++)  K  /♦  read  all  four  driving  motors  sequentially  */ 

267 

268  driveCountPrevious  [ix]  *driveCount  [ix]  ;  /♦  save  previous  value  */ 

269  steerCountPrevious[ix]=steerCount  [ix]  ;  /*  save  previous  value  */ 
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270 

271 

272 

273 

274 

275 

276 

277 

278 

279 

280 
281 
282 

283 

284 

285 

286 

287 

288 

289 

290 

291 

292 

293 

294 

295 

296 

297 

298 

299 

300 

301 

302 

303 

304 

305 

306 

307 

308 

309 

310 

311 

312 

313 

314 

315 

316 

317 

318 

319 

320 

321 

322 

323 

324 

325 

326 

327 

328 

329 

330 

331 

332 

333 

334 

335 

336 

337 

338 

339 

340 

341 

342 

343 

344 

345 


/♦ - 

/*  read  drive  encoders  for  wheel  ix 

/♦ - 

♦(p+3)=0x03; 

*(p+3)=0x01; 


*/ 

♦/ 

*/ 


/* 

/* 


load  output  latch  from  counter  */ 
initialize  two-bit  output  latch  */ 


d-( (unsigned  char*) ftdrive Count [ix] )+2 ; 
*d—  =  *(p+l)  k  OxOOff; 

*d—  «  ♦(p+1)  k  OxOOff; 

*d  =  *(p+l)  k  OxOOff; 


/*  start  with  LSB,  need  offset 
/*  read  LSB  first 
/*  read  next  byte 
/♦  read  most  significant  byte 


*/ 

*/ 

*/ 

♦/ 


/♦ - 

/♦  read  steer  encoders  for  wheel  ix  */ 
/. - 


*(p+0xl03)=0x03; 

/*  load  output  latch  from  counter 

*/ 

*(p+0il03)=0x01; 

/*  initialize  two-bit  output  latch 

*/ 

d= ( (unsigned  char *) ft steerCount [ix] ) +2 ; 

/♦  load  LSB  first 

*/ 

*d—  =  *(p+0xl01)  ft  OxOOff; 

/*  read  LSB  first 

♦/ 

*d—  =  *(p+0xl01)  ft  OxOOff; 

/*  read  next  byte 

♦/ 

‘i  =  *(p+0xl01)  ft  OxOOff; 

/*  read  most  significant  byte 

♦/ 

p=p+4; 

/*  increment  pointer  for  next  motor*/ 

/*  determine  difference  between  previous  and  current  encoder  reading  »/ 
steerDelta  [ix]  =  (steerCount  [ix]  -steerCoimtPrevious  [ix] )  /256 ; 
dri  veDelta  [ix]  =  (driveCount  [ix]  -dr  iveCountPrevious  [ix] )  /256 ; 

>  /*  end  of  for  */ 

/*  account  for  the  fact  that  a  positive  driveDelta  for  wheels  2  and  4  */ 

/*  indicate  that  wheel  is  driving  backwards  !»!  Thus,  change  sign  to  */ 

/*  obtain  a  positive  driveDelta  for  wheel  driving  forwzord  !!!  */ 

driveDelta [l]=-driveDelta[l] ; 
driveDelta [3] --driveDelta [3] ; 

return; 

y  /*  end  of  readEncoder  */ 


cofflputeSpeedAndAngle () 


Environment : 
Neune; 

Last  update: 
Purpose : 


GCC  Compiler  v2.7.2 
Thorsten  Leonardy 
11/21/97 

This  function  computes  the  speeds,  angles  and  angular  velo- 
for  all  four  wheels  based  on  the  most  recent  shaft 
encoder  readings  from  readNewEncoderO  . 


*  Called  from:  driver ()  in  movement. c 

♦  - 

void  computeSpeedAndAngle (void) 


* 

* 

♦ 

♦ 

* 

* 

* 

* 

*/ 


int  i; 


/♦  compute  measured  driving  speed  [cm/sec]  and  steering  angle  [rad]  and  */ 
/*  steering  rate  [rad/sec]. 
for(i=0;  i<=3;  i++)  < 

actualSpeeds [i]  -  ( (double) driveDelta [i] ) *CH_PER_DIGIT/0 , 01 ; 

actualAngles[i]  +=  ( (double) steerDelta [i] )*RAD_PER^DIGIT; 

^  actualAngleRatesCi]  =  ( (double) steerDelta [i] )*RAD_PER_DIGIT/0. 01 ; 

return; 


/* 

/♦  Verifies  validity  of  incoming  speeds/angles  and  converts 
/*  digitial  input  for  the  DA  board 
/* 


♦/ 

*/ 

*/ 

*/ 
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346  void  driveMotors(){ 

347 

348  int  ix,Speed_Digit,Steer_Digit,  counter; 

349  double  speedl,  steerl,  temp; 

350 

351  unsigned  short  bitMask=Ox8000;  /♦  access  bit  15  for  align  wheel  1  ♦/ 

352  unsigned  short  *servoStatus= (unsigned  short  ») (VME9421+0x00ca) ;  /*  digital  input  */ 

353 

354  bitMask  =  bitMask  »  3; 

355 

356  /♦  updateWheelDriveO ;  wheel  values  for  driving  */ 

357  /♦  updateVheelSteerO ;  ♦/ 

358  /♦  comupte  the  current  actual  wheel  direction  in  WheelDirAct []  ♦/ 

359 

360  if  (mode  !=  100)< 

361  for(ix  =0;  ix  <ARRAY_SIZE;  ix++){ 

362  /*  ♦♦♦★♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦★steering/driving  interaction*************  */ 

363  /*  here  +/-  1/50  of  the  steering  value  is  added  to  the  driving  */ 

364  /*  for  each  specified  wheel.  Note  the  negative  sign  on  elements  [1]  */ 

365  /*  and  [3] provide  the  same  direction  driving  as  elements  [0]  and  [2]  */ 

366 

367  Omega^Speed  =  desiredSpeeds[ix]  + 

368  SteerDriveInteract*desiredAngleRates[ix]*18.9;  /*  cm/sec  */ 

369 

370  /*  conversion  to  digits  */ 

371  Speed_Digit  =  velocityReferenceTable(Omega_Speed,lx)  + 

372  DriveFeedBackGain* (Omega_Speed  “  actualSpeeds[ix] ) ; 

373  Steer.Digit  =  rateReferenceTable(desiredAngleRates[ix]) 

374  +  steerFeedbackGain*  (desiredAngleRates  [ix]  -actualAngleRates  [ix]  ) 

375  +  angleFeedbackGain*norm(desiredAngles [ix]  '-actualAngles [ix] ) ; 

376 

377  if  (Speed_Digit>DigitsHigh)  /*  Limitation  ♦/ 

378  Spced_Digit=  Digit sHigh; 

379  if  (Steer.Digit>DigitsHigh) 

380  Steer_Digit=  Digit sHigh; 

381  if  (Speed_Digit<DigitsLow) 

382  Speed_Digit=  DigitsLow; 

383  if  (Steer_Digit<DigitsLow) 

384  Steer_Digits=  DigitsLow; 

385 

386  switch(mode){ 

387  case  2: 

388  case  3: 

389  case  4: 

390  case  5: 

391  case  6: 

392  case  7 : 

393  case  8: 

394  case  9: 

395  case  10: 

396  case  11:  /*  case  11:  linear  test  drive,  added  11/03/97  Leo  */ 

397  speedDigits[ii]=  (short) Speed_Digit;  /*  casting  to  short  */ 

398  steerDigits[ii]=  (short) St eer^Digit; 

399  break; 

400 

401  case  1: 

402  speedl  =  speedDigits [ix] ; 

403  steerl  =  steerDigits [ix] ; 

404  if  (  speedl  >  0)  speedl — ; 

405  if  (  speedl  <  0)  speedl++; 

406  if  (  steerl  >  0)  steerl — ; 

407  if  (  steerl  <  0)  steer 1++; 

408  speedDigits [ix]  =  speedl; 

409  steerDigits [ix]  =  steerl; 

410  break; 

411  }  /*  end  switch  ♦/ 

412  }  /♦  end  for  */ 

413  }  /*  end  if  */ 

414  else  { 

415  for  (ix=0;  ix<3;  ix++)'{ 

416  steerDigits [ix]  =  0; 

417  } 

418  for  (ix-0;  ix<4;  ix++)< 

419  speedDigits [ix]  =  0; 

420  } 

421 


77 


422  switch(modeTstate)-C 

423  case  0; 

424  steerDigitsC3]  =  50»Flag; 

425  modeTstate  -  1; 

426  breeik; 

427 

428  case  1: 

429  modeTstate  =  2; 

430  break; 

431 

432  case  2: 

433  modeTstate  =  3; 

434  break; 

435 

436  case  3: 

437  modeTstate  *  4; 

438  break; 

439 

440  case  4:  - 

441  modeTstate  =  5; 

442  break; 

443 

444  case  5: 

445  modeTstate  =  6; 

446  break; 

447 

448  case  6: 

449  modeTstate  =  7; 

450  breadc; 

451 

452 

453  case  7: 

454  modeTstate  =  8; 

455  breedc; 

456 

457  case  8: 

458  modeTstate  =  9; 

459  break; 

460 

461  case  9: 

462  modeTstate  =  10; 

463  breadc; 

464 

465  case  10: 

466  modeTstate  =  11; 

467  break; 

468 

469  case  11: 

470  modeTstate  =  12; 

471  break; 

472 

473  case  12: 

474  modeTstate  ==  13; 

475  break; 

476 

477  case  13: 

478  modeTstate  =  14; 

479  break; 

480 

481  case  14: 

482  modeTstate  =  15; 

483  break; 

484 

485  case  15: 

486  modeTstate  =  16; 

487  break; 

488 

489  case  16; 

490  modeTstate  =  17; 

491  break; 

492 

493  case  17 : 

494  modeTstate  =  18; 

495  break; 

496 

497  case  18: 
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498  modeTstate  =  19; 

499  break; 

500 

501  case  19: 

502  if  (bitMaskft+servoStatus)/*  read  servo  status »  ♦/ 

503  ■[  /*wait  until  wheel  aligned  */ 

504  Flag  =  -Flag; 

505  modeTstate  =  20; 

506  } 

507  break; 

508 

509  case  20: 

510  steerDigits[3]  =  0; 

511  modeTstate  ^  21; 

512  break; 

513 

514  case  21: 

515  modeTstate  =  22; 

516  break; 

517 

518  case  22: 

519  modeTstate  =  23; 

520  break; 

521 

522  case  23 : 

523  modeTstate  =  24; 

524  break; 

525 

526  case  24: 

527  modeTstate  =  25; 

528  break ; 

529 

530  case  25: 

531  modeTstate  =  26; 

532  break; 

533 

534  case  26: 

535  modeTstate  =  27; 

536  break; 

537 

538  case  27 : 

539  modeTstate  =0; 

540  break; 

541 

542  default  :  break; 

543  1  /*  end  switch  ♦/ 

544  }  /♦  end  else  ♦/ 

545 

546  #ifdef  0 

547  driveSteer (steerDigits) ; 

548  driveSpeedsCspeedOigits) ; 

549  #endif 

550 

551  /*  here  is  a  more  efficient  way  of  setteing  the  speeds  [Leo,  11/18/97]  ♦/ 

552  /*  instead  of  using  the  functions  driveSteer  and  driveSpeeds  ...  «/ 

553  setServoSpeedO ; 

554 

555 

556  }/*  end  driveMotors  */ 

557 

558 

559 

560  double  velocityReferenceTable (double  desiredVelocity,int  i) 

561  { 

562  double  inVelocity, 

563  outVelocity; 

564 

565  inVelocity=new_abs(deBiredVelocity) ; 

566 

567  if  (inVelocity>=0.0  frft  inVelocity<=5.0) 

568  outVelocity  =  inVelocity*Kl [i] ; 

569 

570  if  (inVelocity>5.0  kk  inVelocity<  8.0) 

571  outVelocity  =  inVelocity*K2 [i] ; 

572 

573  if  (inVelocity>=8.0  kk  inVelocity <20.0) 
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outVelocity  =  inVelocity*K3[i] ; 
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if  (inVelocity>=20.0  ftfe  inVelocity<=  70.0) 
outVelocity  =  inVelocity*K4 [i] ; 

if  (inVelocity>70.0  kk  inVelocity<K5) 
outVelocity  =  inVelocity*K6[i] ; 

if  (iiiVelocity>  K5) 
outVelocity=1023 ; 

if  (de8iredVelocity<  0.0) 
outVelocity  =  —  outVelocity; 

return  outVelocity; 

}  /*  end  velocityLookupTable  */ 

double  rateReferenceTable (double  desiredRate) 

double  inRate, 

outDigit ; 

/♦outDigit  =  new_abs  (desiredRate) ;  *//*  test  only  */ 

inRate=new_abs (desiredRate) ; 

if  (inRate<=  5.234) 
outDigit  =  inRate*195.4155  ; 
else 

outDigit“1023 ; 

if  (desiredRate<  0.0) 
outDigit  =  -  outDigit; 

return  outDigit; 

> 


- - 

♦  readOneEncoderO 

* 

*  Environment;  GCC  Compiler  v2.7,2 

*  Name:  Thorsten  Leoneurdy 

*  Last  update;  10/27/97 

*  Purpose:  Reads  only  the  encoder  specified  by  'wheel*; 

*  wheel  =  0  ...  3  reads  drive  encoder  for  wheel  1 . .  4 

*  wheel  =4  ...  7  reads  steer  encoder  for  wheel  1 . . 4 

*  Note;  !!!  The  data  (24  bit)  is  still  left  adjusted  !!! 

void  readOneEncoder(iut  ix»  int  *data) 


* 

* 

♦ 

* 


* 

* 

* 

* 

*/ 


unsigned  char  >t'p,»d; 

p=  (unsigned  char>*‘)VMECTRl; 
p=p+4*ix; 

if  (ix>3)  p=p+0x0090;  /♦  account 

*(p+3)=0x03; 

♦(p+3)=0x01; 

da (unsigned  char  *)data; 
d=d+2; 

♦d~  a  *(p+l)  k  OxOOff ; 

a  ♦(p+l)  k  OxOOff; 

*d  a  ♦(p+l)  ft  OxOOff; 

return; 


/*  access  steering  register  ♦/ 

for  the  fact  VHECTR2aVM(rrRl+0xl00  ♦/ 

/*  load  output  latch  from  coimter  ♦/ 
/♦  initialize  two-bit  output  latch  ♦/ 


/♦  start  with  LSB,  need  offset  ♦/ 

/♦  read  LSB  first  */ 

/♦  read  next  byte  ♦/ 

/♦  read  most  significant  byte  ♦/ 


I  /♦  end  of  readOneEncoder  ♦/ 


/* 
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*  linearHotionO  ♦ 

♦  ♦ 

*  Environment:  GCC  Compiler  v2.7.2  * 

*  Name:  Thorsten  Leonardy  * 

*  Last  update:  10/27/97  ♦ 

*  Purpose:  IMplements  a  linear  motion  test  profile  such  that  the  ♦ 

*  vehicle  is  following  steps  in  successive  lOsec  time  * 

*  intervals .  * 

*  Call:  User  presses  *1*  on  the  keyboard  (see  user()  in  file  user.c)* 

*  - 


void  linearMotionl(void) 

-C 

double  vlx,  vly,  v2,  vlyvlxRatio»omega2,omega3,  beta,ro,ro2»wheelAngleV; 
int  ix,Speed.Digit,Steer_Digit ; 
short  ♦servoOut; 

/*  read  all  shaft  encoders  */ 
readNeuEncoder () ; 

/*  compute  the  actual  rates,  velocities  and  angles  */ 
for  (ix=0;  ix<4;  ix++){ 

drive  Speed  [ix]=driveDe  It  a  [ix]*CM_PER_DIGIT/DELTA_T;  /*  [cm/s]  */ 

steerRate  Cix]*steerDeltaCix] /DELTA_T; 

steerAngle [ix] =steerAngle  [ix] +st eerDelta [ix] *RAD_PER_DIGIT ; 

}  /♦  end  of  for  ...  */ 


/*  initialize  temporary  veiriables  */ 
speed^otlon .  Speed; 
theta=mot ion . Theta ; 
omega=motion . Omega ; 

/* - * 

*  body  motion  (former  in  movement. c)  ♦ 

*  - 

a=2.0;  /*  acceleration  is  2cm/sec'^2  */ 


if  (time<1000)  { 

speed=a^time/100.0;  /♦  rise  linearly  from  0  ..20  cm/sec  in  10  secs  */ 

y 

if  (time^^lOOOX 

speed=a*10.0;  /*  vehicle  speed  constant  for  next  10  sec  */ 

> 

if  (time>=2000) 
if  (time<3000) 

speed=a* (3000-time) /lOO.O;  /*  decelerate  to  zero  speed  for  20sec..30sec  */ 

if  (time==3000)-C 

speed-O.O;  /*  stop  vehicle  for  30sec..40sec  */ 

> 

if  (time>=4000) 
if  (time<5000) 

speed=a*(4000.0-time)/100.0;  /♦  reverse  motion,  move  back  for  40sec  ..  50sec  ♦/ 

if  (time-=5000) i 

speed=-a'»'10.0;  /*  move  back  with  constant  velocity  */ 

} 

if  (time>»6000) 
if  (time<7000) 

speed-a* (time-7000 . 0) /lOO . 0 ; 

if  (time==7000)-C 
mode=0 ; 

stopVHE9325() ;  /*  stop  A/D-Board  ♦/ 

allOffAndZeroO  ; 

> 

/*  compute  required  derivatives  */ 
speedDot- (speed-motion . Speed) /DELTA^T ; 
thet  aDot=  (theta-motion .  Theta)  /DELTA_T ; 
omegaDot=  ( omega-motion .  Omega)  /DELTA_T ; 
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/*  update  the  motion  */ 
motion. Speed  =  speed; 
motion. Theta  =  theta; 
motion. Omega  —  omega; 

/*  update  the  vehicle  configuration  */ 

vehicle. heading  =  vehicle .heading  +  motion. 0mega»DELTA_T; 

vehicle,  coord.  X  =  vehicle .  coord,  x  +  motion.  Speed+DELTAlT**  cos  (motion.  Theta) ; 

vehicle,  coord,  y  =  vehicle .  coord,  y  +  motion.  SpeedOELTA.T  ♦  sin  (mot  ion.  Theta)  ; 

- -  ^ 

♦  drive  motors  (former  in  motor. c)  ^ 

*  _ _ _ 


for  wheel  1  */ 
for  wheel  2  */ 
for  wheel  3  ♦/ 
for  wheel  4  */ 

/*  set  the  speeds  */ 
setServoSpeedO ; 

return; 

1  /*  end  of  leoMotionO  ♦/ 


dd [0] =speed/wheelRadius [0] *16615 . 776 
dd [1] =speed/wheelRadius [1] *16615 . 776 
dd  [2] -speed/wheelRadius [2] *16615 . 776 
dd  [3] =specd/wheelRadius [3] *16615 . 776 


speedDigits [0]=(short) (0. 0132421*dd[0] -1 . 15119) 
speedDigits [1] = (short) (0 . 0132276*ddCl] -1 . 17617) 
speedDigits  C2] = ( short ) (0 . 0132283 tdd [2] +0 . 171 10) 
speedDigits [3]  =  (short) (0 . 0132680*dd [2] +1 . 21652) 


/*  set  speed 
/*  set  speed 
/*  set  speed 
/*  set  speed 


void  linearMotion2(void) 

double  vlx,  vly,  v2,  vlyvlxRatio, omega2,omega3.  beta.ro,ro2,wheelAnKleV* 
int  ix,Speed_Digit ,Steer_Digit ; 
short  *servo0ut; 


/*  read  all  shaft  encoders  ♦/ 
readNewEncoderO  ; 

/*  compute  the  actual  rates,  velocities  and  angles  */ 
for  (ix=0;  ix<4;  ix++){ 

driveSpeed[ix]=driveDelta[ix]*CM^PER_DIGIT/DELTA  T;  /*  [cm/s]  */ 
steerRate  [ix]  =steerDeltaCix]  /DELTA  _T; 
steer  Angle  [ix]=steerAngle[ix]+steerDelta[ix]*RAD_PER  DIGIT- 
}  /*  end  of  for  . . .  */  ’  ’ 


/*  initialize  temporary  variables  */ 
speed=mot ion. Speed; 
t he t  a=mot ion . The t  a ; 
omega=miot  ion.  Omega; 


*  body  motion  (former  in  movement. c)  ^ 

* 

a=100.0;  /*  max  acceleration  Ccm/sec“2]  */ 

/*  no  acceleration  for  t<l8ec  */ 
if  ((time>=100)tfc(time<200)) 

speed=0.005*(tiine-100)*(time-100):  /•  vehicle  speed  [cm/see]  (nav  is  50cm/sec  */ 

if  ( (time>=300)&fe(time<400) ) 

speed=800 . 0+0 . 005*time* (time-800 . 0) ; 

if  (time==400)< 
mode=0; 

stopVME9325();  /*  stop  A/D-Board  */ 
allOffAndZeroO  ; 

> 

/*  compute  required  derivatives  */ 
speedDot= (speed-mot ion . Speed) /DELTA.T ; 
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802  thetaDot- (theta-motion .  Theta)  /DELTA^T ; 

803  omegaDot* (omega-motion . Omega) /DELTA_T ; 

804 

805  /*  update  the  motion  */ 

806  motion. Speed  >  speed; 

807  motion. Theta  =  theta; 

808  motion. Omega  -  omega; 

809 

810  /*  update  the  vehicle  configuration  */ 

811  vehicle  .heading  =  vehicle. heading  +  motion.  0mega*DELTA_T; 

812  vehicle .  coord.  X  -  vehicle,  coord,  x  motion.  Speed*DELTA_T  *  cos  (motion.  Theta) ; 

813  vehicle. coord. y  =  vehicle. coord. y  +  motion. Speed*DELTA_T  ♦  sin(motion.Theta)  ; 

814 

815  /♦ - * 

816  ♦  drive  motors  (former  in  motor. c)  * 

817  4c - */ 

818 

819  dd[03=speed/wheelEadius  [0]  *16615 . 776 ; 

820  dd[l]  =^speed/wheelRadius  [1]  *16615 . 776 ; 

821  dd[23=speed/wheelRadius [2] *16615.776 ; 

822  dd[33=speed/wheelRadius [3] *16615.776 ; 

823 

824 

825  speedDigits[03  =  (short)  (0.0132421*ddC0]-l. 15119) ;  /*  set  speed  for  wheel  1  */ 

826  speedDigits[l]«(short)  (0.0132276*dd[l]-l .  17617) ;  /*  set  speed  for  wheel  2  */ 

827  speedDigits[2]=(short)  (0.0132283*dd[2]+0. 17110) ;  /*  set  speed  for  wheel  3  */ 

828  speedDigits [33  =  (short)  (0.0132680*dd [2] +1 .21652)  ;  /*  set  speed  for  wheel  4  */ 

829 

830  /*  set  the  speeds  */ 

831  setServoSpeedO ; 

832 

833  return; 

834  >  /*  end  of  leoMotion2()  */ 

835 

836 

837  /* - * 

838  *  setServoSpeedO  * 

839  *  * 

840  *  Environment:  GCC  Compiler  v2.7.2  * 

841  *  Name:  Thorsten  Leonardy  * 

842  *  Last  update:  10/27/97  * 

843  *  Purpose:  This  function  sets  the  speed  as  specified  in  global  vars  ♦ 

844  *  speedDigits  and  steerOigits  to  all  servo  motors.  * 

845  *  Called  from:  driver ()  in  movement.c  * 

846  ♦ - - ♦/ 

847  void  setServoSpeed(void) 

848  { 

849 

850  short  *servo0ut=  (unsigned  short*)  (VHE9210+0x0082) ;  /*  Analog  out  */ 

851 

852  *servo0ut++=  -speedDigits  [03  *16;  /*  set  speed  for  driving  wheel  1  */ 

853  *servo0ut++=  speedDigits  [13*16;  /*  set  speed  for  driving  wheel  2  */ 

854  *servo0ut++=  -speedDigits [23 *16;  /*  set  speed  for  driving  wheel  3  */ 

855  *servo0ut++=  speedDigits  [33 ’^16;  /*  set  speed  for  driving  wheel  4  */ 

856 

857  *servo0ut++=  steerDigits  [03  *16;  /*  set  speed  for  driving  wheel  1  */ 

858  *servo0ut++=  steerDigits  [13 *16;  /*  set  speed  for  driving  wheel  2  */ 

859  *servo0ut++=  steerDigits  [23  *16;  /*  set  speed  for  driving  wheel  3  */ 

860  *servo0ut++=  steerDigits  [3]  *16;  /*  set  speed  for  driving  wheel  4  */ 

861 


862  return; 

863  }  /*  End  of  setServoSpeed  */ 

864 

865 

866 


867  /* - * 

868  *  clearEncoder (motors)  * 

869  *  * 

870  *  Environment:  GCC  Compiler  v2.7.2  * 

871  *  Last  update:  03  November  1997  * 

872  *  Name:  Thorsten  Leonardy  * 

873  *  Purpose:  This  function  clears  all  sheift  encoders.  * 

874  *  * 

875  *  motors  bit  mask  to  select  motors,  eg.  0x042  selects  motor  2  and  7  * 

876  *  to  be  cleared.  * 

877  * - - - 
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void  clearEncoder(iinsigned  char  motors) 

unsigned  char  *p= (unsigned  char*)VMECTRl; 
int  ix; 

for  (ix=0;  ix<4;  ix++,motors/=2)  { 

if  (motors  &  0x01)  *(p+3)=0x04;  /♦  dear  respective  counter  ♦/ 

If  (motors  k  0x10)  ♦(p+0x0103)=0x04;  /♦  clear  steering  counter  */ 

^  p=p+4;  access  next  pointer  */ 

return; 

1  /*  end  of  clearEncoder  */ 


- - 

*  alignO 

*  Environment:  GCC  Compiler 

*  Last  update:  07  August  1997  m 

*  Name:  Thorsten  Leonardy  and  Yutaka  Kanayama 

*  Purpose:  This  function  will  align  SHEPHERD^s  wheels  such  that  all 

*  will  point  in  the  forward  direction.  It  utilizes  the  hall 

*  sensors  for  each  of  the  four  wheels.  All  wheels  are  being 

*  aligned  simultaneously  rather  than  successively. 

*  - 

void  align (void) 

< 


* 

* 


* 

* 

* 

* 

* 

*/ 


unsigned  short  *servo0ut= (unsigned  short*) (VHE9210+0x008A) ;  /♦  Analog  out  */ 

unsigned  short  *servoStatU5= (unsigned  short  ♦) (VME9421+0x00ca) ;  /*  digital  input  */ 

unsigned  int  *servoControl= (unsigned  int  ♦)VME2170;  /♦  Data  Out  ♦/ 

int  ix ; 

unsigned  short  bitMask,speed=Ox0200; 


/*  set  steering  speeds  */ 
♦servoOutsf-speed;  /* 

♦  (servoOut+l)s=  speed;  /♦ 

*(servo0ut+2)=  speed;  /* 

*(servoDut+3)=-speed;  /* 


wheel!  ->  rotate  CW  */ 
wheel2  ->  rotate  CCW  */ 
wheels  ->  rotate  CCW  */ 
wheel4  ->  rotate  CW  ♦/ 


bitMask=0xf 000 ; 


while (bitMask){ 

if  (  0x8000  ft  ♦servoStatus  ){ 

*servo0ut=0x0000;  /*  set  speed=0  for  wheel  1  ♦/ 

bitMask=bitMask  ft  0x7000; 

> 

if  (  0x4000  ft  ♦servoStatus  )■( 

♦(servoOut+l) =0x0000;  /♦  set  speed=0  for  wheel  2  ♦/ 

bitHask=bitMask  ft  OxbOOO; 

> 

if  (  0x2000  ft  *servoStatus  ){ 

♦(servoOut +2) =0x0000;  /♦  set  speed=0  for  wheel  3  ♦/ 

bitMask=bitMask  ft  OxdOOO; 

> 

if  (  0x1000  ft  ♦servoStatus  ){ 

♦(servoOut+3) =0x0000;  /♦  set  speed=0  for  wheel  4  ♦/ 

bitMask=bitMa5k  ft  OxeOOO; 

> 

> 

sio0ut(0, "Aligned  ...\n\r"); 
return; 

)■  /♦  end  of  align  ♦/ 


*  all  servos  on  and  set  zero  speed,  [added  11/05/97,  Leo]  ♦ 

♦ 

void  allOnAndZero (void) I 

unsigned  int  *8erToControl= (unsigned  int  »)VHE2170:  /»  Data  Out  ♦/ 

short*)  (VME9210+OX0082) :  /♦  Analog  out  driving  uheell  */ 

for  (ii=0:  ii<8i  ir++)  »servo0ut++=0i0000 ;  /♦  set  zero  speed  »/ 
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954  *servoControl=0i00924924;  /*  turn  on  all  motors  ♦/ 

955 

956  return; 

957  }  /♦  end  of  allOnAndZero  */ 

958 

959 

960  /♦ - ♦ 

961  ♦  all  servos  off  and  set  zero  speed,  [added  11/05/97,  Leo]  * 

962  * - ♦/ 

963  void  allOff AndZero(void)-C 

964  unsigned  int  *servoControl=s (unsigned  int  *)VME2170;  /♦  Data  Out  ♦/ 

965  short  *servo0ut=  (unsigned  short*)  (VHE9210+0x0082)  ;  /*  Analog  out  driving  wheell  */ 

966  int  ii; 

967 

968  for  (ix=0;  ix<8;  ix++)  ♦servoOut ++=0x0000;  /♦  set  zero  speed  ♦/ 

969 

970  *servoControl=0x00000000 ;  /*  turn  on  all  motors  ♦/ 

971 

972  return; 

973  }  /*  end  of  allOffAndZero  ♦/ 

974 

975 

976  /* - ♦ 

977  *  Set  all  driving  motors  to  specific  speed  ♦ 

978  * - ♦/ 

979  void  allDrive (short  digit)  { 

980  unsigned  int  ♦servoControl=  (unsigned  int  ♦)VME2170;  /*  Data  Out  ♦/ 

981  short  *servo0ut=  (unsigned  short*)  (VME9210+0x0082)  ;  /♦  Analog  out  driving  wheell  */ 

982  int  ix; 

983 

984  for  (ix=0;  ix<4;  ix++)  ♦servo0ut++=digit ;  /*  set  zero  speed  */ 

985 

986  *servoControl=0x00000924;  /*  turn  on  driving  motors  ♦/ 

987 

988  return; 


989  }  /♦  end  of  allDrive  ♦/ 

990 

991 

992  /* - ♦ 

993  *  Set  all  steering  motors  to  specific  speed  * 


994  ♦ - 

995  void  allSteer( short  digit) 

996  i 

997  unsigned  int  *5ervoControl=  (unsigned  int  *)VME2170;  /*  Data  Out  */ 

998  short  *8ervo0ut=  (unsigned  short*)  (VHE9210+0x008A)  ;  /*  Analog  out  steering  wheell  ♦/ 

999  int  ix; 

1000 

1001  for  (ix=0;  ix<4;  ix++)  *servo0ut++=digit ;  /*  set  zero  speed  */ 

1002 

1003  *servoControl=0x00924000;  /*  turn  on  steering  motors  */ 

1004 

1005  return; 

1006  }  /*  end  of  allSteer  */ 

1007 

1008 

1009  /♦ - * 

1010  *  switches  all  motors  off  [added  11/05/97,  Leo]  * 

1011  * - ♦/ 

1012  void  allMotorsOf f (void) < 

1013  unsigned  int  *servoControl=  (unsigned  int  *)VHE2170;  /*  Data  Out  */ 

1014 

1015  *servoContro 1=0x00000000;  /♦  turn  off  all  motors  ♦/ 

1016 

1017  return; 

1018  }  /*  end  of  allHotorsOff  */ 

1019 

1020 

1021  /* - * 

1022  ♦  switches  all  motors  on  [added  11/05/97,  Leo]  * 

1023  - - */ 

1024  void  allMotors0n(void)< 

1025  unsigned  int  *servoControl=  (unsigned  int  *)VME2170;  /*  Data  Out  */ 

1026 

1027  ♦servoControl=0x00924924 ;  /*  turn  on  all  motors  »/ 

1028 

1029  return; 
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1030 

1031 

1032 

1033 

1034 

1035 

1036 

1037 

1038 

1039 

1040 

1041 

1042 

1043 

1044 

1045 

1046 

1047 

1048 

1049 

1050 

1051 

1052 

1053 

1054 

1055 

1056 

1057 

1058 

1059 

1060 
1061 
1062 

1063 

1064 

1065 

1066 

1067 

1068 

1069 

1070 

1071 

1072 

1073 

1074 

1075 

1076 

1077 

1078 

1079 

1080 
1081 
1082 

1083 

1084 

1085 

1086 

1087 

1088 

1089 

1090 

1091 

1092 

1093 

1094 

1095 

1096 

1097 

1098 

1099 

1100 
1101 
1102 

1103 

1104 

1105 


y  /*  end  of  allMotorsOn  ♦/ 


/♦ - 

*  driveTestO 

^  - 

*  Environment: 

GCC  Compiler  v2.7.2 

*  Last  update: 

29  October  1997 

*  Name: 

Thorsten  Leonardy 

*  Purpose: 

This  function  computes  the  actual  servo  data  for  all 

* 

driving  motors. 

*  Called  from: 

*  - 

userO  upon  keyboard  interaction  (type  ’d*) 

void  driveTestO 

i 


unsigned  int  *servoControl= (unsigned  int  *)VME2170; 

unsigned  short  *servo0ut= (unsigned  short*) (VME9210+Ox008A) ; 
unsigned  short  *seT  '^tatus=  (unsigned  short  *)  (VME9421+0x00ca)  * 
unsigned  short  bit!  .:=0x8000;  /*  access  bit  15  for  alig^ 

unsigned  char  *p; 
unsigned  int  wheelSelect ; 
int  ix; 


/*  Data  Out  */ 

/*  Analog  out  */ 

/*  digital  input  */ 
wheel  1  ♦/ 


•servoControl=0i00000000;  /♦  disable  (turn  off)  all  wheels  »/ 


servoOut==  (unsigned  short*)  (VME9210+0x0082)  ; 
WheelSelect =0x00000004;  /♦ 


/*  Analog  out  for  drive  wheel  1*/ 
select  servo  for  driving  wheel  1  */ 


p=(unsigned  char*)VHECTRl ; 
for  (ix=0;  ix<4;  ix++)  i 


*servo0ut-test Speed;  /*  set  output  value  for  servo  first 

*servoControl=wheelSelect ;  /*  turn  on  selected  servo  motor 

sio0ut(0 /'Press  » . »  to  start  recording  time\n\r'’)  ; 

while  (key!=*.*)  ;  /♦  wait  until  user  starts  */ 

*(p+3)=0x04;  /*  clear  coxmter  for  driving  wheel  ix  */ 

readDneEncoder(ix,(int  *)ftdriveCountPrevious tix] ) ;  /*  update  encoder  */ 
readOneEncoder(ix,(int  *) ftsteerCount Previous [ix] ) ;  /♦  update  encoder  */ 

timeForTum[ix]=intCounter;  /*  store  time  (start  observing)  */ 

sio0ut(0 /'Press  to  stop  recording  time\n\r’'); 

while  (key !=»,»)  ;  /♦  y^it  until  user  stops  the  process  */ 


timeForTum  [ix]=intCounter-timeForTurn[ix]  ; 


*S€rvo0ut ++=0x0000;  /*  stop  wheel  ♦/ 

readOneEncoder(ix,(int  *)ftdriveCount [ix] ) ;  /*  update  encoder  */ 
readOneEncoder(ix,(int  *) ftsteerCount [ix] ) ;  /*  update  encoder  */ 

driveDelta [ix] = (dr iveCount [ix] -driveCountPrevious [ix] ) /256 ; 
steerDelta [ix] = (steerCount [ix] -steerCount Previous [ix] ) /256 ; 

wheelSelect=  wheelSelect «3;  /*  select  next  servo  (motor)  */ 

> 

*servoControl=0x00000000;  /*  disable  (turn  off)  all  wheels  */ 

return; 

}  /*  end  of  driveTest  */ 

- - - 

*  velocityTestO  ^ 

*  Environment:  GCC  Compiler  v2.7.2  * 

*  Last  update:  07  November  1997  ^ 

*  Name;  Thorsten  Leonardy  ^ 
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1106  ♦  Purpose:  This  function  obtaines  the  velocity  versus  digit  curve.  * 

1107  ♦  Drive  servos  aore  given  different  velociies  (digit)  every  ♦ 

1108  *  two  seconds.  The  first  second  is  to  obtain  steady  state,  the* 

1109  *  second  second  will  record  the  shaft  encoder  difference,  thus* 


1110  ♦  giving  rise  to  a  encoder  reading  versus  velocity  curve.  * 

1111  *  The  commanded  velocity  goes  from  500  ..  -510  at  present.  * 

1112  *  * 

1113  *  Called  from:  userO  upon  keyboard  interaction  (type  ^v*)  * 

1114  * - */ 

1115  void  velocityTest (void) 

1116  { 

1117  unsigned  int  *servoControl=  (unsigned  int  *)VME2170;  /*  Data  Out  */ 


1118  short  *servo0ut= (unsigned  short*) (VME9210+0x0082) ;  /*  Analog  out  driving  wheell  */ 

1119 

1120  short  speed, digit; 

1121 

1122  speed=500; 

1123  digit=speed*16; 

1124 

1125  leoData=(int  *) 0x00100000;  /*  start  data  storage  */ 

1126 

1127  sio0ut(0, ” velocityTest \n\r '•) ; 

1128  align  0  ; 

1129  allOffAndZeroO; 

1130 

1131  *servoControl=0x00000924 ;  /*  turn  on  driving  motors  ♦/ 

1132 

1133  readNewEncoder () ; 

1134  time-0;  /*  this  will  be  altered  by  timer  interrupt  */ 

1135 

1136  /♦  set  new  driving  values  ♦/ 

1137  *servo0ut++=-digit ;  /*  set  speed  for  wheel  1  */ 

1138  *servo0ut++=  digit;  /*  set  speed  for  wheel  2  */ 

1139  *servo0ut++=-digit;  /*  set  speed  for  wheel  3  */ 

1140  *servoOut++=  digit;  /*  set  speed  for  wheel  4  */ 

1141 

1142  while  (speed>“510)  < 

1143 

1144  servo0ut= (short  *) (VME9 2 10+0x0082 ) ; 

1145 

1146  /*  set  new  driving  values  ♦/ 

1147  ♦servo0ut++=-digit ;  /*  set  speed  for  wheel  1  */ 

1148  *servo0ut++=  digit;  /*  set  speed  for  wheel  2  ♦/ 

1149  *servo0ut++ss-digit ;  /*  set  speed  for  wheel  3  ♦/ 

1150  *servoOut++=  digit;  /♦  set  speed  for  wheel  4  */ 

1151 

1152  speed=speed'-10; 

1153  digit=speed*16;  /♦  shift  nibble  left  */ 

1154  time=0; 

1155 

1156  /♦  wait  a  second  for  motors  to  settle  ♦/ 

1157  while (time<100)  ; 

1158 

1159  readNewEncoder () ; 

1160 

1161  /♦  record  for  a  second  */ 

1162  while (time<200)  ; 

1163 

1164  readNewEncoderO ; 

1165 

1166 

1167  /*  store  the  counter  data  for  previous  speed  */ 

1168  *leoData++=steerDelta[03 ; 

1169  ♦leoData++=steerDeltaCl] ; 

1170  »leoData++=steerDelta[2] ; 

1171  *leoData++=steerDelta [3] ; 

1172  *leoData++=dxiveDelta[0] ; 

1173  *leoData++=driveDelta[l3 ; 

1174  *leoData++=driveDelta[23 ; 

1175  *leoData++=driveDelta[3] ; 

1176  > 

1177 

1178  allOffAndZeroO; 

1179 

1180  retxim; 

1181  }  /*  end  of  velocityTest  */ 
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*  circmnferenceTest  () 


*  Environment:  GCC  Compiler  v2.7.2  ^ 

*  Last  update:  07  November  1997  ^ 

*  Name:  Thorsten  Leoneirdy  ^ 

*  Purpose:  This  function  drives  the  vehicle  in  a  straight  line  and  * 

*  stores  the  difference  for  all  shaft  encoders  for  a  given  ♦ 

*  observation  time.  If  the  distance  travelled  is  being  ♦ 

*  measi^ed,  one  can  obtain  the  relation  between  shaft  encoder  * 

*  readings  and  wheel  diameter.  ^ 

*  Called  from:  user()  upon  keyboard  interaction  (type  ’c*)  4c 

void  circumferenceTest(void) 

< 

unsigned  int  *servoControl=  (unsigned  int  ♦)VME2170;  /♦  Data  Out  */ 
short  ♦servoOut=  (unsigned  short*)  (VME9210+0x0082)  ;  /*  Analog  out  driving 

short  speed, digit; 

speed=300; 

digit=speed*16; 

leoData=(int  *) 0x00100000;  /♦  start  data  storage  */ 

sioOut (0, "circumf erenceTest () \n\r") ; 
alignO ; 

allOffAndZeroO; 


*servoControl=0x00000924 ; 


/*  turn  on  driving  motors  */ 


/*  determine  the  digits  to  command  based  on  linea4r  relationship  obtained  ♦ 

*  in  velocityTest  for  each  wheel  individually.  */ 

/*  assume  for  one  second,  that  driveDelta=10000  */ 


/*  set  new  driving  values  for  driveDelta  approx  10000  over  1  sec  ♦/ 


*servo0ut++= (short ) (-16* (0 . 0132421*ddc-l . 15119) ) 

♦servo0ut++= (short) (  16* (0 . 0132276*ddc-l . 17617) ) 

*servo0ut++:=(short)  (-16* (0.0132283*ddc+0. 17110)) 

*servo0ut++=( short) (  16* (0.0132680*ddc+l. 21652)) 

time-0;  /♦  this  will  be  altered  by  timer  interrupt  */ 

readNewEncoderO  ; 


/*  set  speed  for  wheel  1  */ 
/*  set  speed  for  wheel  2  */ 
/*  set  speed  for  wheel  3  */ 
/*  set  speed  for  wheel  4  •/ 


while  (time<tc)  ; 
readNewEncoderO ; 
allOffAndZeroO  ; 


/*  wait  2  sec  */ 


return; 

}  /*  end  of  circumferenceTest  ♦/ 


♦  steerTestO 


*  Environment: 

*  Last  update: 

*  Name: 

*  Purpose: 

* 

*  Called  from: 


GCC  Compiler  v2.7.2 
29  October  1997 
Thorsten  Leoneurdy 

This  function  computes  the  actual  servo  readings  for  all 
steering  motors. 

userO  upon  keyboard  interaction  (type  ’wO 


void  SteerTestO 
•C 

unsigned  int  ♦servoControl=  (unsigned  int  *)VHE2170;  /*  Data  Out  ♦/ 

unsigned  short  * servo Out = (unsigned  short*) (VHE9210+Ox008A) ;  /*  Analog  out  */ 

unsigned  short  *servoStatus= (unsigned  short  *) (VME9421+0x00ca) ;  /*  digital  input  */ 

Tinsigned  char  *p;  o  r 
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unsigned  short  bitMask^OzSOOO;  /*  access  bit  15  for  align  wheel  1  */ 
unsigned  int  wheelSelect=Ox00004000;  /♦  select  servo  for  turning  wheel  1  ♦/ 
int  ix. turns, a; 


1258 

1259 

1260 
1261 

1262  /♦  align  wheels  ♦/ 

1263  alignO ; 

1264 

1265  /★  clear  all  driving  and  steering  motor  counters  and  the  variables  */ 

1266  clearEncoder(Oxff ) ; 

1267 

1268  s ervo Out  =  (unsigned  short*)  (VME9210+Ox008A)  ;  /♦  Analog  out  for  steering  wheel  1  ♦/ 

1269  bitMask=0x8000 ;  /*  access  bit  15  for  align  wheel  1  */ 

1270  wheelSelect=0x00004000;  /*  select  servo  for  turning  wheel  1  ♦/ 

1271 

1272  readNewEncoderO ;  /*  read  all  encoders  */ 

1273 

1274  for  (ix=0;  ix<4;  ix++)  < 

1275 

1276  turns«0; 

1277  *servoOut=test Speed;  /*  set  output  value  for  servo  first  ♦/ 

1278  *servoControl5=wheelSelect;  /*  turn  on  selected  servo  motor  */ 

1279 

1280  /*  turn  wheels  for  a  total  of  10  turns  */ 

1281  do  i 

1282  while  (!  (bitHask&*servoStatus)) ;  /*  wait  until  wheel  aligned  */ 

.1283  while (bitMask&*servoStatus) ;  /*  wait  until  wheel  progressed  */ 

1284  tums++;  /*  one  turn  completed  */ 

1285  if  (tums==l) 

1286  timeForTumCix]=intCounter;  /♦  store  time  (start  observing)  */ 

1287  if  (tums==9){ 

1288  timeForTum[ix]=(intCounter-timeForTum[ix3)/8;  /*  stop  timer  */ 

1289  *servo0ut++ss0x0800;  /*  speed  for  final  turn  ♦/ 

1290  } 

1291  >while  (turns<10) ; 

1292 

1293  wheelSelect=  wheelSelect«3 ;  /*  select  next  servo  (motor)  */ 

1294  bitMask  =  bitMask  »  1;  /*  select  ner  xt  status  align  bit  */ 

1295  > 

1296 

1297  ♦servoContro 1=0x00000000;  /*  disable  (turn  off)  all  wheels  */ 

1298 

1299  readNewEncoderO; 

1300 

1301  for  (ix=0;  ix<4;  ix-f^)  radPerDigitCix] *2- 0*PI*10.0/ (double) stcerDelta[ix]  ; 

1302 

1303  return; 

1304  }  /♦  end  of  steerTest  */ 


1305 

1306 

1307  /♦ - * 

1308  ♦  stopTestO  * 

1309  *  * 

1310  *  Environment:  GCC  Compiler  v2.7.2  ♦ 

1311  *  Last  update:  03  November  1997  ♦ 

1312  ♦  Name:  Thorsten  Leonardy  * 

1313  *  Purpose:  This  function  computes  the  actual  servo  readings  for  all  * 

1314  *  steering  motors  while  the  motor  speeds  are  set  to  zero.  * 

1315  *  Called  from:  userO  upon  keyboard  interaction  (type  ’s*)  * 

1316  * - ♦/ 

1317  void  stopTestO 

1318  { 

1319 


1320  sio0ut(0, "Aligning  Wheels  ...\n\r"); 

1321 

1322  align 0;  /♦  align  wheels  */ 

1323 

1324  /*  clear  all  driving  and  steering  motor  counters  emd  the  variables  */ 

1325  clearEncoder(0xff ) ; 

1326 

1327  readNewEncoderO; 

1328  allOnAndZeroO; 

1329 

1330  time=0; 

1331  sio0ut(0, "Please  Wait  a  minute  .,.\n\r"); 

1332  while  (time<6000)  ;  /*  wait  a  minute  */ 

1333  allOffAndZeroO: 
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1334 

1335  sioOut  (0,  "Done\n\r'’) ; 

1336  readNewEncoderO ; 

1337 

1338  return; 

1339  }  /♦  end  of  stopTest  #/ 

1340 

1341 

1342 

1343 

^***********************************************************************nf**** 
1345  End  of  motor. c 
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APPENDIX  D:  SHEPHERD  PRIMER 


This  appendix  provides  essential  data  and  procedures  which  lead  to  the  findings  of  the 
motion  parameters  that  are  required  to  operate  SHEPHERD  properly.  Boxed  text  will  refer  to  a 
segment  of  software  code  or  a  command  sequence  for  use  in  the  TAURUS  Debugger  environment. 
The  focus  is  on  the  use  of  the  TUARUS  Debugger  since  this  provides  a  quick  way  to  determine  most 
of  the  operating  parameters. 


1.  MAIN  OPERATING  PARAMETERS  AND  CONVERSION  FACTORS 

It  is  sometimes  tedious  to  gather  the  meat  for  operating  a  system.  This  section  strives  to 
provide  most  of  the  operating  parameters  pertaining  to  the  use  of  SHEPHERD  in  tabulated  form. 


Wheel  Radius 
max.  Tire  pressure 

0.189  m 

49.8  psi 

Drive  Encoder  (all  Wheels) 

2  TT  radians  =  360  *  290  counts 

1  m  =  87914  counts 

1  count  =  11.37 

Wheel  1 

digit  =  187.20  v  [cm/sec]  -  26.4 

Wheel  2 

digit  =  187.04  v  [cm/sec]  -  26.4 

Wheel  3 

digit  =  186.88  v  [cm/sec]  -  4.8 

Wheel  4 

digit  =  187.20  v  [cm/sec]  +  8.8 

Steer  Encoder  (all  Wheels) 

2  TT  radians  =  360  *  256  counts 

1  degree  =  256  counts 

Table  4.1:  Shepherd  Operating  Parameters  in  a  Nutshell 


91 


2. 


RESET  AND  READ  SHAFT  ENCODERS 


To  find  out  how  the  servo  readings  relate  to  either  the  steering  and/or  the  driving,  use 
the  following  debugger  sequence  which  resets  the  servo  counter  for  one  wheel,  drives  the  wheel  and 
reads  the  servo  counter  after  steering  is  done.  The  same  procedures  would  apply  for  use  with  the 
remaining  servo  motors. 


Taurus _Bug>m8  ffffSlOb  04 
Taurus _Bug>BB  Tfff048B  0800 
Taurus^Bug>BS  ffffffOO  00100000 


Taurus. Bug>mB 
Taurus. Bug>Bs 
Tauru8_Bug>m8 
Taarus.Bug>B<l 
FFFF6109  D3 


ffffffOO  00000000 
ffffOlOb  03 
ffffeiOb  01 
ffff6109;l;b 


Taurus _Bug>»d  ffff6109:l;b 
FFFF6109  C6 

Taurus, Bug>md  ffff6i09:l;b 

FFFF6109  FB 

Tauru8_Bug> 


t  clear  servo  coustar  for  steering  wheel  3 
t  set  velocity  for  steering  wheel  3 
t  turn  on  sotor  for  steering  wheel  3 

*  . . .  after  a  certain  nuiaber  of  revolutions  . . . 
k  turn  off  notor  for  steering  wheel  3 

*  select  control  for  motor  7  (steer  wheel  3) 
t 

k  read  least  significant  byte  of  24bit  counter 
k  the  result 
k  read  next  byte  . . . 
k  ,  the  result 
k  read  nost  significant  byte 
4  . . .  the  result 

ft  the  complete  counter  value  in  this  case  is 
ft  0xfbc6d3  sign-extended  (e.g.  -276781) 


3.  UP-  AND  DOWNLOADING  DATA  FROM  TAURUS  BOARD 

At  this  time,  there  is  no  straight  forward  routine  for  data  up-  and  downloading  available. 
Hence,  the  up-  and  downloading  of  data  such  as  waypoints,  ...  is  very  tedious.  The  only  way,  data 
can  be  transferred  from  or  to  the  TAURUS  main  memory  is  via  the  TAURUSBug  options  ‘du'  for 
downloading  data  to  the  Laptop  and  ‘lo’.  However,  data  would  be  made  available  only  in  form  of 
the  Motorola  S-Record  format. 

To  download  data  from  the  TAURSU  main  memory  to  the  Laptop,  the  Laptop  must  capture 
the  script  sent  to  the  screen  to  a  file  (option  ”T”ext  ”C”apture  on  the  menu  bar),  fri  a  second  step, 
output  the  data  to  the  screen  using  the  folowing  command: 


TauruB_Bug>du0  iOOOOO  lOOOff  ‘This  Is  a  dump  to  the  screen* 
Effective  address:  00100000 
Effective  address:  OOlOOOff 

S01F000064686973206973206120647S6D7020746F2074686S20736372666BSEF1 

S21410000012341234123412341234123412341234AB 

S214100010123412341234123412341234123412349B 

S21410002012341234123412341234123412341234BB 

S214100030123412341234123412341234123412347B 

S214100040123412341234123412341234123412346B 

S214100050123412341234123412341234123412346B 

S214100060123412341234123412341234123412344B 

S214100070123412341234123412341234123412343B 

S214100080123412341234123412341234123412342B 

S214100090123412341234123412341234123412341B 

S214 1000 A01234 12341234 1234 12341234 123412340B 

S2i41000B012341234l23412341234123412341234FB 

S2141000C012341234123412341234i23412341234EB 

S21410000012341234123412341234123412341234DB 

S2141000E012341234123412341234123412341234CB 

S2 141000F01234 12341234 1234 1234 1234 12341234BB 

S9030000FC 

TauniB,Bug> 
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As  can  be  seen  above,  the  data  from  memory  location  0x100000  to  OxlOOOf  f  will  be  output 
to  the  screen  and  thus  captured  in  the  ascii  file  specified.  However,  the  data  will  be  in  the  Motorola 
S-Record  format  and  a  parsing  program  needs  to  extract  the  pure  data.  The  parsing  program 
however,  needs  to  know  the  datatype  of  the  data  given  to  extract  the  correct  information.  E.g., 
extracting  data  of  datatype  ‘integer’  would  require  a  diflFerent  parsing  routine. 

As  far  as  the  uploading  of  data  is  concerned,  the  datafile  must  be  transferred  in  the  same 
manner  as  the  SRK  program,  with  the  ‘LO’  option  and  described  by  [1]. 


4.  INTERRUPTS 

This  section  describes  briefly  what  type  of  interrupts  are  enabled  on  SHEPHERD. 


a.  Timer  Interrupt 

Every  10  ms,  a  timer  interrupt  is  issued  by  the  on  board  timing  circuit.  The  interrupt 
handling  routine  ’TimerHandler’  does  the  following: 

1.  increments  counter  ’intCounter’ 

(which  may  be  needed  for  timing  purposes) 

2.  initiate  (software  trigger)  a  block  conversion  for  the  A/D-Board  AVME9325-5 

3.  call  function  ’driver’  in  file  ‘movement .c’  to  execute/handle  motion  control  part 

The  interrupt  is  routed  through  the  Interrupt  steering  mechanism  (ISM)  to  the  VIC068  and 
from  there  to  the  68040  processor  in  the  following  way: 


IACK-3 


b.  A/D-Board  Interrupt 

Every  10  ms,  the  timer  circuit  initiates  the  start  of  a  block  conversion  on  the  A/D-Board. 
Once  this  conversion  is  complete,  the  A/D-Board  AVME9325-5  issues  an  interrupt  to  indicate  that 
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the  conversion  is  complete  and  data  is  available  to  be  read  from  its  dual  port  RAM.  The  interrupt 
handler  ‘handlerVME9325()’  then  subsequently  calls  ’analyzeData’  to  further  analyze/process 
the  data.  The  interrupt  vector  number  is  provided  by  the  Board  and  set  to  be  0x0056  which  relates 
to  the  location  of  the  address  for  interrupt  handling  routine  at  0x0158  in  the  interrupt  vector  table. 

As  opposed  to  on-board  interrupts,  the  interrupt  from  the  A/D-Converter  VME  board  is 
routed  directly  through  the  VIC068  to  the  68040  processor: 


VMEBus 


IRQ-2 

VIC068 

IRQ-2 

68040 

c.  Keyboard  Interrupt 

The  overarching  framework  for  user  interaction  is  provided  by  the  routine  ’user()’  in  file 
’user.c’.  Each  time,  the  keyboard  is  pressed,  an  interrupt  is  issued  by  the  68C681  on  board  serial 
circuit  to  the  68040  through  the  TSM  and  VIC068.  The  ascii  code  for  the  key  pressed  is  then  be 
stored  in  the  variable  inPortA  ana  further  analyzed  by  the  routine  ‘user()’  in  61e  ‘user.c’.  The  mode 
flags  set  in  this  function  will  be  further  processed  by  functions  called  during  the  motion  control  cycle 
following  each  10ms  timer  interval.  For  this  interrupt,  the  interrupt  vector  number  is  provided  by 
the  DUART  and  set  to  be  0x0060  thus  giving  rise  to  the  location  of  the  interrupt  handling  routine 
InPortAHandler  at  0x0180  in  the  interrupt  vector  table. 


IACK-1 


5.  REPRESENTATION  OF  DOUBLE  VARIABLES 

According  to  the  M68040  users  manual,  any  double-precision  variable  is  stored  in  memory 
as  an  8  byte  data  value  in  the  following  form 

Since  the  representation  is  normalized  with  the  leading  (implicit)  bit  always  one  we  find  the  relation 
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Bit  63  =  s  =  sign  bit  (l=negative  number) 

Bit  62. .52  =  e  =  11  bit  exponent  in  the  range  0x000  . . .  0x7f  f 
Bit  51..0  =  f  =  52  bit  (13  nibbles)  binary  decimal  (mantissa) 
_ in  the  range  0x0000000000000  . . .  Oxf ff f f f f f f f ff f 


to  the  real  number  representation  x  by 

X  =  (-1)*  (1  +  d) 

with  d  =  /  •2'“^^  .  As  an  example,  to  display  the  double  variable  stored  in  memory  location  0x306e8 
we  issue  the  following  TAURUSbug  commands 


Taurus _Bug>md  306e8 : 1 ; d 
000306E8  1_3F1^1DF44179E4364 

The  result  is  conveniently  displayed  by  the  monitor  such  that  the  elements  can  be  easily  identified: 
s=l,  e=0x03f  1,  f=0xldf44179e4364.  Hence,  the  real  number  is 

^  ^  0x10000000000000'^ 


6.  HOW  TO  RUN  SHEPHERD’S  WHEELS 


Three  VME  boards  account  for  operating  of  the  wheels,  both  in  steering  and  driving.  These 
boards  are  accessible  via  the  VME  Bus  Port  connector  PI  and  they  are: 


Board 

Function 

GCC  Access 

VME  9210 

Analog  Output  to  servos  (velocity) 

short 

VME  2170 

Servo  Control  (on/off) 

unsigned  int 

VME  9421 

Servo  Status 

unsigned  short 

Shepherd  is  equipped  with  a  total  of  eight  servo  motors:  four  wheels  with  driving  and 
turning  capability.  The  setup  and  software  configuration  is  depicted  in  Figure  (1).  In  order  to 
operate  each  one  of  the  motors  one  has  to  perform  the  following  steps: 

1.  Select  the  angular  velocity  for  the  motor  by  writing  a  signed  short  value  (16  Bit)  to  the 
respective  channel  (see  Figure  1  for  the  channel  assignments)  on  the  VME9210  board  (analog 
Output).  E.g.  to  turn  wheel  3  (rear  right)  one  would  write 


* (f f f f 048e) = (short ) velocity ; 
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where  a  positive  velocity  corresponds  to  the  spin  direction  as  indicated  by  the  arrow  in  Fig. 
(1).  The  well  known  Right-Hand  rule  applies  for  determining  the  direction  of  spin. 

2.  Switch  the  motor  on/offby  writing  the  respective  mask  to  VME2170  at  OxffffffOO.  Refer 
to  Fig.  (1)  for  the  mask  assignment.  E.g.  to  drive  wheel  2  (front  left)  and  turn  wheel  4 
(rear  left)  simultaneously,  one  would  issue  the  command 


*(OxffffffOO)=(unsigned  int) 0x00800020 


Any  combination  is  allowed,  i.e.  mask  0x00900000  would  turn  wheels  3  and  4.  Make  sure 
you  have  set  the  angular  velocities  for  the  wheels  you  are  going  to  run  as  outlined  in  step  1 
above! 


A  word  of  Caution:  for  driving  wheels  1  (front  right)  and  3  (rear  right)  forward,  negative  values  must  be 
written  to  the  VME9210  Board  as  outlined  in  step  1. 
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o 


Wheel  2 

drive 

steer 


Channel 

0xffff0484 

0xffff048c 


Mask 

0x00000020 

0x00020000 


Wheel  4 

drive 

Steer 


Channel 

0xffff0488 

0xffff049Q 


Mask 

0x00000800 

0x00800000 


O 


Front 


Shepherd 


Switchbox  (aft) 


Wheel  1 

drive 

steer 


Channel 


Mask 


0xffff0482  0x00000004 

0xffff048a  0x00004000 


Wheel  3 

drive 

steer 


Channel 

0xffff0486 

0xffff048e 


Mask 

0x00000100 

OxQOlOOODQ 


Figure  4.1:  Wheel  Assignment  and  Servo  Register  Addressing  (Arrows  and  Dots  at  each  wheel 
indicate  the  rotation  of  the  respective  servos  if  controlled  with  positive  values. 
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